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ABSTRACT 

Underwater  walking  machines  offer  a  potential  for  replacement  of  human  divers  in 
certain  aspects  of  underwater  construction  and  inspection  One  such  vehicle,  Aquarobot, 
is  currently  under  test  in  Japan  However,  this  vehicle  is  currently  too  slow  to  be 
economically  utilized,  and  limited  hardware  availability  restricts  progress  in  control 
software  improvements.  A  software  dynamic  simulation  model  is  desirable  to  relieve  thus 
restricted  access  The  problem  addressed  by  this  research  is  the  modeling  of  system 
dynamics  of  underwater  walking  vehicles  with  sufficient  simplification  to  achieve  a 
real-time  simulation.  The  approach  taken  includes  an  object-oriented,  massless  leg  robot 
dynamic  model  and  employs  a  high  performance  graphics  rendering  toolkit. 

The  resulting  simulations  of  a  robotic  joint  actuator  and  of  the  robot  itself,  utilizing 
springs  and  dampers  in  the  joints,  runs  in  real-time.  The  robot  simulation  model  executes 
on  a  four-processor  machine  with  under  fifteen  percent  utilization  of  the  processor 
dedicated  to  system  dynamics.  This  result  indicates  that  the  simulation  is  likely  to  retain 
real-time  capability  after  replacing  the  springs  and  dampers  with  the  more  accurate  joint 
actuator  model  also  developed  in  this  thesis. 
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I.    INTRODUCTION 

A.  AQUAROBOT 

Aquarobot  is  a  six-legged  "insect  type",  articulated,  experimental  prototype  robot 
under  development  at  the  Port  and  Harbour  Research  Institute  (PHRI)  in  Japan  This 
robot  is  being  investigated  as  an  alternative  to  the  currently  used  human  divers  for  seawall 
construction  and  inspection.  While  the  divers  are  fully  capable,  the  limited  stay  time  at 
required  depths  make  progress  slow  and  expensive  A  walking  robot  is  preferred  to 
tracked,  wheeled,  or  floating  versions  for  its  abilities  to  maneuver  without  disturbing  the 
bottom  enough  to  cloud  the  water  and  restrict  visibility,  and  to  provide  a  stable  reference 
platform  from  which  measurements  can  be  made  [Iwasaki,  1987]  The  prototype 
Aquarobot  has  successfully  walked  underwater  and  demonstrated  functional  feasibility  for 
the  intended  task,  but  it  is  currently  too  slow  to  be  economically  utilized  [Davidson, 
1993].  The  Naval  Postgraduate  School  (NPS)  is  working  with  PHRI  to  upgrade 
Aquarobot's  control  software,  from  the  original  version  written  in  BASIC,  to  improve  its 
operating  speed. 

B.  GOALS 

The  goal  of  this  thesis  is  to  investigate  the  feasibility  of  dynamic  modeling  of 
Aquarobot  with  sufficient  simplifications  to  achieve  a  real-time  simulation.  The  simulation 


model    should    be    statically    accurate    and    dynamically    approximate.       The    major 
simplifications  considered  are: 

(1)  massless  legs, 

(2)  body  mass  evenly  distributed  in  a  cylinder, 

(3)  center  of  mass  at  geometric  center  of  the  inboard  leg  joints, 

(4)  infinite  friction  for  foot  contact  with  surface, 

The  dynamic  Aquarobot  model  of  this  thesis  uses  springs  and  dampers  in  place  of 
joint  actuators  for  this  initial  feasibility  study.  A  joint  actuator  simulation  model,  including 
servomotor  and  controller  models,  is  also  developed  and  is  intended  to  eventually  replace 
the  springs  and  dampers.  Inputs  to  that  model  will  be  control  software  orders  to  the  joint 
motor  controller. 

C.  ORGANIZATION 

Chapter  II  of  this  thesis  reviews  previous  and  concurrent  work  in  the  area  of  walking 
robots  with  emphasis  on  work  related  to  Aquarobot.  Chapter  HI  provides  a  more  detailed 
description  of  Aquarobot  and  introduces  the  software  tools  used.  Chapters  IV  and  V 
develop  the  necessary  mathematical  models  and  then  present  prototype  dynamic 
simulation  models  for  an  Aquarobot  joint  actuator  and  for  Aquarobot  itself  (with  springs 
and  dampers  in  place  of  joint  actuators).  Chapter  VI  reviews  the  results  of  simulations 
accomplished  with  the  models  introduced  in  Chapters  IV  and  V.  Finally,  Chapter  VTI 
presents  some  conclusions,  suggestions  for  further  research,  and  a  summary. 


II.    SURVEY  OF  PREVIOUS  AND  CONCURRENT  WORK 

A.  INTRODUCTION 

To  place  Aquarobot  research  in  relative  perspective,  this  chapter  begins  with  a  brief 
historical  review  of  walking  machines.  An  overview  of  ongoing  Aquarobot  research  at 
NPS  follows  and  places  this  thesis  in  context  Other  contributions,  some  completed  and 
some  currently  in  progress,  are  described  Also,  as  this  thesis  is  a  continuation  of  previous 
work,  a  short  review  of  some  of  the  key  elements  of  that  work  is  presented  to  provide  a 
starting  frame  of  reference. 

B.  BRIEF  HISTORY  OF  WALKING  MACHINES 

In  an  early  exploration  of  walking  mechanisms,  1965  to  1968,  General  Electric 
Corporation  built  a  four  legged  vehicle  called  the  Quadruped  Transporter.  Because  of  the 
lack  of  theory  and  technology,  designers  incorporated  human  sensing  and  neural  control  of 
the  limbs  by  attaching  "position  following,  force  feedback"  control  levers  to  the  operator's 
arms  and  legs.  Each  of  these  levers  had  three  degrees  of  freedom,  corresponding  to  those 
of  the  leg  it  controlled.  Very  few  mastered  the  skills  required  to  operate  the  vehicle,  and 
those  that  did  found  it  to  be  very  demanding  [McGhee,  1985].  While  the  Quadruped 
Transporter  lacked  practicality,  its  successful  implementation  encouraged  further  research 


Automation  of  low  level  tasks,  such  as  individual  limb  control,  leaves  the  operator 
free  to  concentrate  on  higher  level,  "supervisory  control"  of  the  vehicle.  In  1977,  this 
method  of  control  was  used  in  the  Ohio  State  University  (OSU)  "Hexapod  Vehicle."  The 
operator  controlled  vehicle  speed  and  direction,  using  a  joystick,  while  limb  motion 
control  and  coordination  was  handled  by  computer.  This  machine  was  utilized  in  the 
development  of  gait  algorithms  [McGhee,  1985]. 

[McGhee,  1986]  addresses  the  energy-efficiency  issue  of  limb  control  and  introduces 
a  method  used  to  achieve  a  cyclic  leg  motion  without  requiring  the  reversal  of  drive 
motors.  This  approach  was  demonstrated  in  MELCRABs  1  and  2  at  Mechanical 
Engineering  Laboratory  in  Japan. 

From  1981  to  1986,  the  Adaptive  Suspension  Vehicle  (ASV)  was  constructed  and 
tested  at  OSU.  The  ASV  was  a  six-legged  vehicle  designed  for  outdoor  operation  on 
irregular,  unmapped  terrain  and  included  a  self  contained,  onboard  power  supply.  It 
carried  an  operator  who  exercised  supervisory  control  via  a  joystick  and  keypad.  Leg 
coordination  and  foothold  selection  were  fully  automated;  the  latter  was  allowed  by 
employment  of  extensive  environmental  sensors  including  an  optical  terrain  scanner 
[Waldron,  1986].  Related  later  work  [Kwak,  1990]  explores  the  use  of  rule-based  limb 
motion  coordination  to  implement  a  "free,"  non-periodic,  gait  permitting  on-line 
optimization  of  foot  placement. 


C.  AQUAROBOT  RESEARCH  AT  NPS 

Aquarobot  research  at  NPS  is  divided  into  two  concurrent  phases  control  and 
simulation  The  first  of  these,  control,  consists  of  Aquarobot  control  software 
development  The  second  phase,  simulation,  involves  development  of  a  graphical 
computer  simulation  of  the  Aquarobot  hardware.  The  simulator  is  required  for  the  final 
stages  of  the  control  software  development,  including  testing. 

1.  Control  Software  Development 

While  final  development  and  testing  of  control  software  depends  the  availability 
of  the  simulation  model,  some  work  has  been  accomplished  prior  to  such  availability  In 
[Schue,  1993]  an  algorithm  is  presented  for  statically  stable,  alternating  tripod  gait 
planning  and  foot  path  planning  for  smooth  leg  motion  during  walking  on  flat  terrain 
Further  developments  in  gait  planning  algorithms  and  demonstration  of  alternative  gaits, 
which  allow  variable  direction  and  speed  but  require  continuous  adjustment  of  leg  liftoff 
and  touchdown  sequence,  are  reviewed  in  [Yoneda,  1993]. 

2.  Simulation  Software  Development 

The  framework  for  the  Aquarobot  simulation  model  is  provided  in  [Davidson, 
1993],     in     which     an     object-oriented     kinematic     model     is     developed  Both 

Danevit-Hartenberg  (DH)  and  Craig  (Modified  Danevit-Hartenberg  or  MDH)  methods  are 
presented  and  then  compared.  The  fundamental  difference  in  the  two  methods  is  in  the 
coordinate  systems  used  for  a  "link,"  the  rigid  limb  component  between  two  joints    The 


DH  methodology  utilizes  the  outboard,  closer  to  end-effector,  joint  as  the  coordinate 
system  origin  while  the  Craig  method  uses  the  inboard,  closer  to  body,  joint.  The  Craig 
version  of  Davidson's  kinematic  model  is  used  in  this  thesis. 

[Goetz,  1994]  explores  a  variety  of  enhancements  for  the  Aquarobot  simulation 
model  A  graphics  model,  which  incorporates  a  surrounding  operating  environment 
(terrain),  is  developed  to  replace  the  original  stick  figure.  The  model  includes  I/O  control 
interfaces  (i.e.  keyboard,  joystick,  spaceball)  and  foot/ground  collision  detection. 

A  complete,  unsimplified  physical  dynamic  simulation  model  of  Aquarobot, 
including  the  hydrodynamic  forces  of  its  operating  environment,  is  also  being  developed 
[McMillan,  1993],  While  this  simulation  is  not  expected  to  run  in  real  time,  it  will  provide 
valuable  data  for  comparison  to  the  simplified  model. 

D.  REUSED  SOFTWARE 

As  mentioned  above,  the  Aquarobot  simulation  presented  in  this  thesis  is  based  on  the 
model  described  by  [Davidson,  1993].  A  summary  of  the  key  features  of  that  model  is 
provided  here  for  quick  reference. 

1.  Rigid  Body  Class 

In  both  LISP  and  C++  versions  of  the  Aquarobot  model,  system  dynamics  for  six 
degrees  of  freedom,  three  translational  and  three  rotational,  are  handled  within  a  "rigid 
body  class"  from  [Davidson,  1993].  System  state  variables  include  world  coordinate 
position  and  orientation,  stored  in  a  4x4  "body  to  world"  coordinate  transformation 
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matrix,  called  the  "H-matrix",  and  velocities  in  body  coordinates  Euler  integrations  are 
used  for  dynamic  updates    Acceleration  equations  in  body  coordinates  are 

u  =  vr-Wq  +  ^-gsmQ ,  (2.1) 

v  =  wp-ur  +  -£+gcosQ  sin(j) ,  (2.2) 

vv  =  uq-  v/?  +  ^+gcos0  cos(|),  (2.3) 

(24) 
(2.5) 

(2.6) 

where  m  is  body  mass;  g  is  gravitational  acceleration,  in  world  coordinates,  /^  /  and  /.. 
are  the  moments  of  inertia; /=  (fx,fy,fz)  is  the  vector  of  applied  forces;  T=  (L,  M,  N)  is  the 
vector  of  applied  torques;  theta  and  phi  are  Euler  pitch  and  roll  angles,  respectively,  u,  v, 
w  are  the  components  of  translational  velocity;  and  p,  q,  r  are  rotational  rate  components 
[Frank,  1969].  With  a  single  exception,  g,  the  above  values  are  expressed  in  body 
coordinates.  The  dynamic  update  is  achieved  by  determining  incremental  position  and 
orientation  changes,  in  body  coordinates,  and  using  those  to  generate  an  incremental 
motion  matrix  which  is  then  post-multiplied  with  the  body's  H-matrix,  and  using  that  result 
to  update  (replace)  the  H-matrix.  Euler  integrations  and  equations  2.1  through  2.6  are 
used  to  update  the  velocity  state  variables  [Davidson,  1993] 


2.  Kinematic  Model 

Each  Aquarobot  limb  (leg)  is  made  up  of  a  series  of  links:  the  inboard  end  of  the 
series  being  physically  connected  to  the  robot's  body  and  the  outboard  end  the  foot.  Using 
the  Craig  method,  a  coordinate  system  at  the  outboard  end  of  a  link  is  described  relative  to 
the  coordinate  system  at  the  inboard  end  by  a  transformation  matrix  called  a  "T-matrix". 
The  T-matrix  depends  on  the  physical  construction  of  the  link  and,  in  the  case  of  rotary 
links,  the  rotation  angle  of  the  inboard  joint.  The  origin  of  each  such  coordinate  system  is 
the  position  of  the  joint  between  two  such  links,  and  the  z-axis  is  aligned  so  that  a  joint 
rotation  is  a  z-axis  rotation.  The  entire  system  is  kept  in  a  hierarchical  structure  of  "rigid 
bodies",  with  H-matrix  updates  done  from  the  top  down:  i.e.  a  link's  H-matrix  may  be 
updated  only  after  the  link  next  inboard  is  updated  (the  robot  body  in  the  case  of  the 
inboard  end  of  each  leg)  [Davidson,  1993]. 

[HJ  =  [H„]  [TJ  (2.7) 

E.   SUMMARY 

The  development  of  the  Aquarobot  simulation  model  will  directly  support  final 
development  and  testing  of  Aquarobot  control  software.  The  dynamic  model  developed  in 
this  thesis  is  based  on  a  previously  developed  kinematic  model.  Before  describing  the 
dynamic  model,  a  more  detailed  description  of  Aquarobot  and  an  overview  of  the  software 
tools  used  is  needed  and  is  provided  in  Chapter  IQ. 


III.    DESCRIPTION  OF  AQUAROBOT  VEHICLE  AND 
SIMULATION  ENVIRONMENT 


A.  INTRODUCTION 

This  chapter  provides  a  physical  description  of  Aquarobot,  including  some  details  that 
are  beyond  the  scope  of  models  developed  in  this  thesis.  Special  emphasis  is  given  to  the 
joint  actuators  as  they  are  the  primary  feature  to  be  modeled.  In  addition,  the  software 
tools  used  in  the  development  are  introduced. 

B.  PHYSICAL  DESCRIPTION  OF  AQUAROBOT 

Figure  3.1  is  a  photograph  of  Aquarobot  which  has  a  body,  that  is  generally 
cylindrical  in  shape,  and  six  legs,  equally  spaced  at  sixty  degrees  apart  Mounted  on  top  of 
the  body  is  a  camera  boom  with  three  rotary  joints  for  positioning.  The  boom  is  equipped 
with  an  ultrasonic  ranging  device  to  assist  in  the  scaling  of  measurements  Within  the 
body  are  two  inclinometers  (for  attitude  sensing),  a  gyrocompass,  and  a  depth  sensing 
device.  Aquarobot  is  computer  controlled  from  the  surface  via  a  four  centimeter  diameter 
tether  cable  attached  to  the  top  of  the  body  The  cable  carries  control  signals  to  the  robot 
and  returns  sensor  signals  back  to  the  controlling  computer. 

Each  of  the  six  identical  insect  type  legs  has  three  rotary  joints,  which  are  driven  by 
the  joint  actuators  described  in  the  next  section,  and  a  freely  rotating  ball  joint  to  attach 
the  disc  shaped  foot.     Each  foot  has  a  pressure  sensitive  touch  sensor  to  provide  an 


indication  of  ground  contact.   Figure  3.2  illustrates  the  axis  of  rotation  for  each  joint  in  an 
Aquarobot  leg. 


Figure  3.1 
Photograph  of  Aquarobot 
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Joint2 
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Figure  3.2 
Aquarobot  Leg  Construction 
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C.  AQUAROBOT  JOINT  ACTUATORS 

Figure  3.3  is  a  block  diagram  of  an  Aquarobot  joint  actuator  Control  software  sends 
incremental  rotation  orders  to  the  controller  as  pulses,  with  polarity  indicating  the  desired 
direction  of  rotation.  Motor  response  is  fed  back  to  the  control  software  as  pulses  in  the 
same  manner  These  feedback  pulses  are  produced  by  a  pulse  generator  on  the  motor 
shaft.  Additional  signals  to  and  from  the  control  software  include  pulse  counter  (PC) 
overflow,  pulse  counter  clear,  and  driver  enable. 


order 


pulses 

Control 

Software 

Interface 


Overflow/Clear 


PC 


D/A 


FA/ 


Enable 


Controller 


Amp 


Amp 


Driver 


Current 
Pickup 


PWM 


feedback 


pulses 


PG 


I 


Motor 


RG 


Joint  Shaft 


Figure  3.3 
Aquarobot  Joint  Actuator 

The  difference  between  the  actual  and  ordered  position,  the  error  signal  of  the  motor, 
is  kept  in  the  pulse  counter.  Orders  increment  the  counter,  plus  or  minus,  depending  on 
desired  direction;  response  pulses  decrement  it.    One  hundred  pulses  are  required  for  one 
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motor  revolution  (determined  by  the  motor  shaft  pulse  generator  output).    The  counter 
overflows  if  the  maximum  count,  +/-  6144  pulses,  is  exceeded. 

A  digital  to  analog  converter  (D/A)  produces  a  DC  signal  directly  proportional  to  the 
current  count  in  the  pulse  counter,  and  its  output  is  the  motor  driver's  primary  input.  That 
is,  the  larger  the  error,  the  higher  the  voltage  applied  to  the  motor  to  correct  the  error.  A 
maximum  count  of +/-6144  is  converted  to  +/-  10VDC,  i.e. 

DA0U,  =  count  *(  10/6 144).  (3.1) 

A  high  gain  for  the  error  signal  is  desirable  for  fast  response  and  for  sufficient 
response  to  small  errors.  However,  by  itself,  the  high  gain  would  cause  severe  overshoot 
and  oscillations  in  the  motor.  Degenerative  feedback,  proportional  to  motor  speed,  is 
used  to  prevent,  or  at  least  minimize,  this  overshoot.  This  degenerative  feedback  is 
provided  by  the  frequency  to  voltage  converter  (F/V)  which  monitors  the  pulse  generator 
output.   The  output  is  3VDC  per  thousand  RPlvTs,  so 

FVoul  =  RPM*  (3/1000)  (3.2) 

The  driver  amplifies  outputs  from  the  D/A  and  F/V  converters  to  produce  the  source 
voltage  ( Vs)  for  the  servomotor.  So  far,  we  have 

Vs  =  GxDAout-G2FVoul,  (3.3) 

where  G,  and  G2  are  respective  gains.   However,  the  driver  is  actually  more  complex  and 
includes  an  additional  internal  feedback  path. 

The  signals  form  the  D/A  and  F/V  converters  are  amplified  and  fed  into  the  pulse 
width  modulator  (PWM)  with  the  F/V  signal  inserted  between  inverting  amplifiers  to 
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provide  degeneration.  A  current  pickup  on  the  PWM  output  line  provides  a  signal 
proportional  the  current  drawn  by  the  motor,  and  therefore  proportional  to  the  motor 
torque.  Recall  that  for  a  given  voltage  applied  to  a  motor,  it  has  a  limited  speed  due  to 
back  EMF.  As  the  motor  approaches  that  speed,  it  draws  less  current  This  current 
(torque)  feedback  signal  provides  regenerative  feedback  in  the  driver  during  motor 
acceleration,  thus  reducing  the  response  time.  Adding  the  torque  feedback  to  Equation 
3.3  vields 

Vs  =  G\DAoul-G2FVout+GiIa,  (3.4) 

where  I a  represents  the  motor's  armature  current  Actually,  this  equation  still  neglects 
some  complexities  in  the  Aquarobot  joint  controller.  Not  shown  in  Figure  3.3  are 
feedback  capacitors  around  amplifiers  which  further  modify  the  equation  for  J  These 
effects  are  not  modeled  in  the  work  of  this  thesis. 

The  motor  is  driven  by  a  squarewave  rather  than  a  DC  voltage.  Figure  3  4  illustrates 
idealized  signals.  The  function  of  the  PWM  is  to  provide  a  squarewave  of  constant 
amplitude,  zero  to  +/-  75  volts,  with  a  duty  cycle  proportional  to  the  input  signal,  so  \\  in 
Equation  3.4  is  actually  an  average  value.  Motor  response  to  this  signal  is  very  close  to  its 
response  to  a  DC  voltage  equal  to  the  average  voltage  of  the  squarewave.  Applying 
Signals  1  or  2  of  Figure  3.4  to  the  motor  is  thus  equivalent  to  applying  +25VDC  or 
-25VDC,  respectively.  This  methodology  is  used  primarily  for  its  efficiency  advantage 
over  a  DC  linear  amplifier  [Truxal.  1958], 
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Figure  3.4 
PWM  Outputs  For  33  %  Duty  Cycles 

Attached  to  each  servomotor  is  a  harmonic  reduction  gear,  built  together  as  a  single 
unit.  The  model  used  for  joint  one  in  each  leg,  RA-20,  has  a  reduction  ratio  of  161:1, 
while  that  used  for  joints  two  and  three,  RH-25,  has  a  160: 1  ratio.  In  addition,  joints  two 
and  three  have  a  bevel  gears  cascaded  with  the  harmonic  gears  with  3:1  and  2:1  ratios, 
respectively.  Figure  3.5  summarizes  the  total  reduction  ratios  and  gives  the  control  pulses 
required  for  a  single  joint  shaft  revolution. 
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Total  Reduction  Gear  Ratios  and 

Control  Pulse  Requirement  per  Joint  Revolution 


14 


D.   SOFTWARE  TOOLS 

C++  is  the  programming  language  selected  for  modeling  Aquarobot.  This  choice  was 
based  on  both  hardware  and  software  considerations  IRIS  Performer,  a  C  C  -  -  \isual 
simulation  toolkit,  provides  rendering  that  is  fast  enough  to  display  a  real  time  dynamic 
simulation  [Goetz.  1994].  Common  LISP  was  used  for  rapid  prototyping  and  testing 
during  the  early  stages  of  development. 

1.  C++ 

In  a  long  term  project  with  many  contributors,  object  oriented  design  provides  a 
high  level  of  abstraction  which  promotes  rapid  system  level  comprehension  by  new  team 
members  as  well  as  loosely  coupled,  easily  maintained  source  code.  Not  only  does  C++ 
support  the  object  oriented  design  paradigm,  but  also,  it  is  based  on,  and  includes  as  a 
subset,  the  highly  efficient,  structured  language.  C  [Wiener,  1988]. 

The  simulation  platform,  both  here  and  at  PHRI,  is  an  IRIS  Reality  Engine  The 
C/C++  compilers  delivered  with  the  IRIS  systems  are  very  efficient,  due  to  direct  access  to 
the  low  level  hardware,  and  are  intended  to  be  used  as  native  development  languages. 

Popularity  and  widespread  use  make  C/C++  source  code  portable,  and  while 
portability  is  not  a  key  issue  for  the  Aquarobot  simulation,  it  is  very  much  so  for  the 
Aquarobot  control  software  which  is  likely  to  have  to  survive  hardware  upgrades 
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2.  Performer 

IRIS  Performer  is  used  primarily  for  its  performance  as  a  rendering  tool. 
Performer  is  a  hardware  oriented.  C/C++  graphics  tool  kit  designed  to  operate  on  Silicon 
Graphics  products.  Its  routines  take  full  advantage  of  the  "known  hardware"  platform  to 
allow  much  higher  performance  than  routines  written  for  generic  hardware.  Also,  some 
precision  is  traded  for  speed  as  real  numbers  in  Performer  data  structures  are  single 
precision  "floats"  rather  than  "doubles".  Upon  initialization.  Performer  detects  hardware 
capability  and  automatically  sets  up  a  multiprocessing  environment  with  shared  memory 
when  running  on  a  multiprocessor  machine.  The  default  configuration,  which  the  user 
may  override,  is  separate  processors  assigned  for  (1)  user  application.  (2)  graphics 
database  culling,  and  (3)  drawing  the  culled  database  to  a  graphics  window  [SGI,  1992]. 

Performer's  graphics  database  is  stored  in  a  hierarchical  data  structure,  a  tree. 
The  structure  is  culled  and  rendered  by  doing  a  pre-order  traversal  with  child  nodes 
inheriting  the  accumulated  environments  (transformations)  of  all  ancestors  traversed  in  the 
current  path.  These  accumulated  environments  are  kept  on  a  stack  and  are  popped  off 
when  traversing  back  up  the  tree.  The  basic  nodes  are  Scene  (the  root),  Static  Coordinate 
System  (SCS),  Dynamic  Coordinate  System  (DCS)  (variable  for  motion  where  an  SCS  is 
fixed),  Group  (a  container  for  children  requiring  a  common  transformation),  and  Geode  (a 
container  for  polygons  to  be  rendered).  Geodes  must  be  leaves  but  may  have  more  than 
one  parent.  This  reduces  the  memory  requirements  when  a  group  of  polygons  is  to  be 
rendered  more  than  once  in  a  frame  (two  or  more  identical  objects)  [SGI,  1992]. 
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Robotics  and  mechanics  users  must  be  aware  that  graphics  standards  are  used  in 
matrix  construction  and  operations.  The  graphics  matrix  is  the  transpose  of  the  standard 
robotics  matrix  [Fu.  1987],  GMt/  =  RMjt.  This  is  only  important  vshen  using  the  pfMatrix 
data  type  and  operations  outside  those  that  directly  support  the  graphics  database,  or 
when  directly  manipulating  individual  elements,  rows,  and  or  columns.  Since  \1\.\ 
(jV'xA/1 )',  the  order  of  matrix  multiplications  may  also  have  to  be  reversed.  Lastlv.  X  and 
Y  axis  rotations  are  as  expected;  however  in  Performer,  pitch  refers  to  an  X.  rather  than 
Y,  axis  rotation,  and  likewise,  roll  refers  to  a  Y  axis  rotations  [SGI,  1992]. 

Synchronization  for  a  real-time  application  may  be  achieved  by  setting 
Performer's  "desired  frame  rate"  and  then  using  the  pfSync  function  call.  The  pfSync 
function  will  put  all  processes  to  sleep  between  each  frame  to  force  the  desired  frame  rate 
If  the  desired  frame  rate  is  faster  than  can  be  achieved,  pfSync  will  have  no  effect,  and  the 
application  will  free  run  at  its  fastest  speed.  In  the  Aquarobot  simulation,  the  frame  rate  is 
set  to  twenty  frames  per  second  and  then  a  fixed  delta-time  of  one  twentieth  of  a  second  is 
used  for  dynamic  updates.  Reading  the  internal  clock  for  delta-time  would  have  been 
equivalent  providing  the  application  is  capable  of  running  at  the  desired  frame  rate.  The 
fixed  delta-time  ensures  control  over  data  points  [SGI.  1992]. 

3.  LISP 

The  problem  with  "rapid,  experimental"  prototyping  in  C/C++,  as  well  as  other 
compiled  languages,  is  the  difficulty  with  testing  and  verification.  Specifically,  a  test  shell 
must  be  written,  compiled  and  run  to  thoroughly  test  a  block  of  source  code.    If  the  test 
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results  in  the  detection  of  errors,  debugging  tools  are  available,  but  the  source  code  must 
be  recompiled  to  include  them.  Other  methods,  such  as  insertion  of  additional  lines  of 
code,  are  also  available  but  also  require  multiple  compilations.  In  any  case,  logic  errors 
are  still  difficult  to  find  when  they  exist  in  large  blocks  of  code,  especially  when  they  only 
apply  to  specific  inputs.  LISP,  on  the  other  hand,  is  an  ideal  language  for  experimental 
work.  LISP  is  an  interpreted,  functional  language  which  gives  the  developer  the  ability  to 
call  any  defined  function  directly  from  the  command  prompt.  The  function's  return  value 
is  immediately  displayed  for  easy  comparison  to  expected  values;  test  routines  are  not 
required.  Since  functions  may  be  nested,  these  test  calls  may  include  any  desired  level  of 
abstraction.  Variables  are  also  directly  accessible  in  LISP  and  may  be  inspected  at  any 
time.  The  basic  structure  of  a  LISP  program  is  an  on-line  database  of  definitions: 
constants,  functions,  and  symbols  (pointers  to  variables)  [Koschmann,  1990]. 

Weak  typing  in  LISP  allows  additional  flexibility.  Typing  is  done  dynamically  at 
run  time.  For  example,  a  single  definition  of  the  function  "minimum''  may  be  used  for 
integers,  reals,  or  any  argument  type  for  which  the  operator  "<"  is  defined.  Variables, 
arguments,  and  return  values  may  also  be  lists,  allowing  the  developer  to  call  a  function 
with  a  comprehensive  set  of  test  inputs  [Koschmann,  1990]. 

The  Common  LISP  Object  System,  CLOS,  provides  full  support  for  object 
oriented  programming.  In  this  thesis,  a  prototype  design  using  CLOS  is  first  developed  to 
implement  system  design  decisions.  Subsequent  translation  to  C++  is  then  accomplished 
without  abstract  structure  modifications. 
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E.   SUMMARY 

This  chapter  physically  describes  Aquarobot  with  the  intention  of  providing  sufficient 
orientation  prior  to  the  model  presentations.  Several  Aquarobot  features  are  not  included 
in  the  dynamic  simulation  model  presented  later  in  Chapter  V.  The  kinematic  model 
includes  the  body,  legs  and  feet  but  not  the  camera  boom.  Joint  position,  foot  contact, 
and  azimuth  information  are  available;  however,  attitude  and  depth  are  not  The  tether 
cable  and  hydrodynamic  forces,  currents  and  viscosity,  are  also  neglected. 

Also  in  preparation  for  the  model  presentations,  the  software  tools  utilized,  along 
with  the  reasons  for  their  selections,  are  introduced.  Object-oriented  system  design  and 
the  need  for  a  high  performance  made  C++  and  IRIS  Performer  ideal  software  tools  for 
the  Aquarobot  simulation  model.  CLOS,  with  its  capability  for  rapid  testing  of  complex 
functions,  was  used  for  prototyping  and  model  verification. 

The  following  chapters  present  dynamic  simulation  models  of  an  Aquarobot  joint 
actuator  and  Aquarobot  itself. 
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IV.     JOINT  ACTUATOR  iMODEL 

A.  INTRODUCTION 

The  purpose  of  this  chapter  is  to  present  a  simulation  model  for  a  single  Aquarobot 
joint  actuator.  It  begins  with  a  review  of  the  basic  mathematics  required  to  model 
servomotors  and  reduction  gears  and  then  develops  the  joint  actuator  model. 

B.  BASIC  DC  MOTOR 

A  motor  is  a  device  used  to  convert  electrical  energy  into  mechanical  energy.  The 
force,  F,  on  a  current,  /,  carrying  conductor  of  length  /  in  a  uniform  magnetic  field,  B,  is 
given  by  [Halliday,    1981] 

F  =  \idlxB.  (4.1) 

If  the  conductor  is  fixed  on  a  shaft,  parallel  to  the  shaft,  at  radius  r,  the  resulting  torque, 
tau,  is  given  by  [Halliday,    1981] 

x=rxF.  (4.2) 

The  motor's  armature  includes  a  set  of  such  conductors  and  is  usually  constructed  so  as  to 
be  symmetrical  with  respect  to  the  axis  of  rotation;  therefore,  the  total  conductor  length 
and  relative  position  (to  the  magnetic  field)  is  independent  of  the  armature's  angular 
position.    Furthermore,  if  a  constant  magnetic  field  (i.e.  permanent  magnet  or  constant 
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current  electromagnet)  is  used,  then  the  force  and  resulting  torque  become  directly 
proportional  to  the  armature  current,  simplifying  the  above  equations  to 

F=iC,  (4.3) 

and 

zd  =  iKt,  (4.4) 

where  taud  is  developed  torque,  and  torque  constant  Kt  is  a  characteristic  of  the  motor. 
The  armature  current  depends  on  applied  voltage,  armature  resistance,  and  armature 
angular  velocity,  omega.  The  velocity  dependency  is  due  to  the  voltage  induced  in  the 
conductors  as  they  move  through  the  field  (Faraday's  Law).  Since  this  induced  voltage  is 
of  such  polarity  that  it  causes  a  decelerating  torque  (Lenz's  Law),  it  is  called  Back  or 
CounterEMF.  Vb  [Halliday.    1981]  [McPherson,  1981] 

l'b  =Kba.  (4.5) 

where  Kh  is  the  motor's  back  EMF  constant. 

Armature  inductance  is  usually  negligible  for  high  quality  servomotors,  so  using  Ohm's 
Law,  the  armature  current,  Ia,  is  given  by  [Halliday,    198 1]  [McPherson,  198 1  ] 

la  =  ^,  (4.6) 

where  V a  is  the  voltage  applied  to  the  armature  and  Ra  is  armature  resistance. 
Combining  Equations  4.4  through  4.6,  the  motor's  developed  torque  is 

Td  =  IaKt  =  K'(V£"a).  (4.7) 

Given  no  external  torques  and  ignoring  losses  for  now,  for  any  Va  there  is  an  associated 
maximum  speed,  when  Va  =  Kh  omega,  that  results  in  taud  =  0  and  therefore  no  further 
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acceleration.  Still  ignoring  losses,  motor  acceleration,  omega-dot.  is  given  by  the  standard 
rotational  dynamics  equation  [Halliday.    1981] 

£  =  22=!  =  ^,  (4.8) 

inertia         ) ,„+■].% 

where  Jm  is  internal  motor  inertia,  and  taux  and  J  are  external  torque  and  inertia, 
respectively.  There  are  several  sources  for  losses  in  a  motor,  but  as  long  as  the  motor  is 
operated  within  its  prescribed  limits,  most  of  them  may  be  consolidated  into  two  groups: 
constant,  Fc,  and  directly  proportional  to  velocity,  Fv  omega.  Some  examples  are  friction, 
copper  (rR).  and  windage  [McPherson,  1981].  External  loads  may  also  include  constant 
and  velocity  dependent  losses.  The  actual  loss  is  state  dependent  and  requires  some 
special  handling: 

(1)  shaft  turning  (omega  <>  0)  :  loss  opposes  omega 

\loss\=Fc  +  Fv\a\;  (4.9) 

(2)  shaft  at  rest  (omega  =  0) :  loss  opposes  torque 

(a)  torque  sufficient  to  overcome  friction  (|torque|  >  Fc) 

loss  =  Fc\  (4.10) 

(b)  torque  insufficient  to  overcome  friction  (requires  torque  -  loss  =  0) 

loss  =  torque .  (4.11) 

State  2a  may  be  handled  by  the  method  used  for  state  1,  but  since  it  is  already  detected  by 

the  test  for  state  2b,  there  is  no  reason  to   perform  the  arithmetic.    Incorporating  losses 

into  Equation  4.8  yields 

•         xj+Zj-Losses  ,.    ,~. 


22 


The  only  other  significant  loss  not  fitting  into  one  of  the  groups  above  is  the  voltage  drop 
across  the  motor  brushes,  brush  drop  loss  /  M.   The  voltage  applied  to  the  motor  armature. 
Va,  is  the  motor  source  voltage.  l\  minus  this  brush  drop  loss  [McPherson.  1981]. 
Va=  Vs-Vbd  (4.13) 

Combining  Equations  4.7,  4. 12  and  4. 13.  motor  response  depends  on  construction,  state, 
applied  voltage,  and  load  (external  torque  and  inertia). 


K, +  Tx  -  Losses 


CO  = 


■Jm   +  I, 


(4  14) 

As  a  final  note,  motors  are  given  voltage  and  load  ratings.  These  are  determined  by 
motor  construction  and  are  intended  to  keep  armature  current  within  safe  operating  limits 
[McPherson.  1981]. 

C.   REDUCTION  GEAR 

A  reduction  gear  is  a  mechanical  coupling  device  that  provides  a  mechanical 
advantage  allowing  a  higher  speed,  lower  torque  source  to  drive  a  lower  speed,  heavier 
load.  This  enables  both  source  and  load  to  operate  at  or  near  their  optimal,  most  efficient 
speeds,  even  though  those  speeds  are  not  the  same.  The  gear  ratio.  //,  is  the  ratio  of  the 
input  and  output  shaft  angular  displacements,  velocities  and  accelerations,  theta,  omega 
and  omega-dot,  respectively  [Chen,  1993]. 

e„u,  =  ^r,  (4.15) 

CW  =  ^r.  (4.16) 

©„„,=¥•  (4-17) 
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While  the  output  shaft  speed  is  reduced  by  a  factor  of//,  the  torque  is  increased  by  a  factor 
of//  [Chen.  1993]. 

t„„,  =//t„,  (4.18) 

Recalling  and  rearranging  Equation  4.8  and  applying  it  to  the  output  shaft 

J «*  =  ¥*-.  (4.19) 


'nut 


Substituting  for  omega-dot mil  and  tauoat,  using  Equations  4. 17  and  4. 18 

J om  =  tt1^  (4.20) 

=  //2^  (4.21) 

co,„ 

=  n2JiH  (4.22) 

This  gives  a  coupling  factor  for  inertia  of//2,  i.e. 

Combining  a  motor  and  a  reduction  gear  as  a  drive  train  for  some  load.  Equations  4. 12, 
4. 18  and  4.22,  yields: 

•  xj +(Zj/n)  -  Losses  ,.    _  ., 

com  = -, — -— ,  or  (4.24) 

or 

•  (n  t d)  +  t. s  -  {n  Losses)  .  .   _  , 
©jr    =  5 "s ,                                                                                                           (4.25) 

(jm  *2J  +./, 
where  m  is  the  input  (motor)  side  of  the  reduction  gear,  and  x  is  the  output  side. 
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D.  JOINT  ACTUATOR  SIMULATION 

Figure  4  1  is  a  block  diagram  of  the  Joint  Actuator  Simulation  Model  Refer  to 
Figure  3.3  for  a  comparison  to  the  actual  Aquarobot  Joint  Actuator  Since  the  model  was 
in  essence  a  "bench  top"  device,  physical  limitations,  such  as  pulse  counter  overflow  and 
motion  limits,  were  not  detected  or  enforced.  Also,  input  pulses  were  replaced  bv  a 
change  in  desired  angular  position  of  the  joint  shaft  in  revolutions,  delta-theta.  and  the 
motor  driver's  rectangular  wave  output  is  simplified  to  its  average  value.  Loading  and 
operating  instructions  and  a  complete  source  code  listing  for  this  model  may  be  found  in 
Appendix  A. 
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Figure  4. 1 
Joint  Actuator  Simulation  Model 


The  D/A  converter  is  not  necessary  in  the  simulation  model  as  the  Pulse  Counter 
stores  the  difference  between  the  current  and  ordered  shaft  positions  rather  than  a  discrete 
pulse  count.    Motor  velocity  and  armature  current,  slots  values  in  the  motor  class,  are 
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directly  accessed  to  provide  those  inputs  to  the  driver.  The  cascaded  amplifiers  in  the 
driver  are  separated  and  then  summed  in  order  to  simplify  gain  adjustments.  Positional 
feedback  is  taken  after  the  reduction  gear  to  match  the  implementation  of  the  positioning 
orders  input.  This  is  in  contrast  to  the  physical  device  in  which  feedback  pulses  come 
directly  from  the  motor  itself. 

1.  Base  Classes 

Several  base  classes  are  used  in  the  joint  actuator  model.  Refer  to  Appendix  A 
for  a  source  code  listing.  The  first  is  the  "diff-counter"  class  used  to  model  the  pulse 
counter  A  single  slot,  "current- count",  which  is  initialized  to  zero,  holds  the  cumulative 
sum  of  all  delta  theta  orders  and  feedback.  Its  single  method,  "diff- signal",  updates  and 
returns  the  current  count. 

count  =  count  +  order  -feedbac  (4.26) 

The  "amplifier- clipper"  class  has  three  slots:  "amplification-factor"  (or  gain), 
"max-value"  and  "min-value".  The  "amplify"  method  simply  multiplies  the  argument  by 
the  gain  and  returns  the  product,  clipped  of  course,  to  the  maximum  or  minimum  value  if 
the  product  exceeds  those  prescribed  limits. 

The  "driver-class",  a  sub-class  of  amplifier- clipper,  adds  three  gain  slots  for 
independent  amplification  of  the  three  inputs  before  summing  them  in  the  final 
amplifier-clipper  stage.  The  current  feedback  is  not  internal  but  relies  on  a  saved  slot 
value  in  the  motor-class. 
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The  "shaft"  class  is  used  as  a  superclass  for  the  motor  class,  as  slots  for  reduction 
gear  input  and  output  shafts,  and  for  the  actual  joint  shaft.  Slots  angular-position  (theta). 
angular-velocity  (omega),  inertia,  coulomb-friction-constant  (Fc  or  constant  loss 
component),  viscous-friction-constant  (Fv  or  velocity  dependent  loss  component),  and 
time-stamp  are  defined.  Methods  are  defined  to  provide  capability  to  set  theta  and  omega 
to  some  position  and  speed,  reset  theta  and  omega  to  zero,  and  couple  (transfer)  theta  and 
omega  to  another  shaft. 

The  "motor"  class  inherits  from  the  shaft  class  and  defines  additional  slots: 
torque-constant  (A,),  back- emf- const  ant  (A'fc),  armature-resistance  (RJ,  armature- current 
(ij,  and  brush  drop  parameters  {max-brush-drop  and  half-brush-drop-source -value). 
Methods  "developed-torque"  and  "omega-dot"  are  direct  implementations  of  Equations 
4.7  and  4.14.  respectively.  Brush  drop  was  approximated  by  using  an  exponential  form 
that  approaches  max-brush-drop  as  the  source  voltage  increases  [McPherson.  1°8 1  J. 

Vbd  =  rWmax  (1-0.5  IWmm*—)  (4.27) 

Method  "run-motor"  gets  the  elapsed  time  (dt),  calculates  omega-dot,  then  updates  the 
motor  state  using  Euler  integrations. 

The  "reduction-gear"  class  has  slots  "gear-ratio"  (n),  "in-shaft"  and  "out-shaft", 
the  last  two  being  instances  of  class  shaft.  Methods  are  provided  to  multiply  or  divide  an 
argument  by  n  (n  squared  if  the  argument  is  inertia).  The  method  "rg-connect"  is  provided 
to  replace  the  shaft  couple  method  for  internal  coupling  and  reduces  the  coupled  theta  and 
omega  by  the  gear  ratio. 
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2.  Joint  Class 

The  "joint"  class  actually  represents  a  joint-actuator.  It  includes  the  entire 
system:  motor  controller,  servo-motor  (prime- mover),  and  reduction-gear.  An  additional 
shaft  slot,  "load-shaft",  is  actually  only  included  to  allow  a  convenient  method  to  store  the 
previous  theta  value  in  order  to  determine  the  feedback  delta  theta.  The  output  shaft  of 
the  reduction  gear  actually  holds  the  remaining  load  shaft  slot  values.  The  motor  could 
likewise  have  been  the  reduction  gear  input  shaft;  however,  they  were  kept  separate  for 
clarity. 

Methods  are  provided  to  sum  system  parameters  external  to  the  motor  (i.e. 
load-inertia,  ...)  so  they  may  be  passed  into  calls  to  run-motor  "Feedback"  returns  the 
difference  between  the  output  shaft  of  the  reduction  gear  and  the  load  shaft. 
"Increment-joint"  calls  "run-motor"  then  couples  all  the  shafts,  being  careful  to  save  the 
old  reduction  gear  output  shaft  position  first  for  use  in  next  call  to  feedback. 
"Step-input-to-joint"  provides  the  facility  to  send  delta  theta  orders  to  the  pulse  counter, 
and  "reset-joint"  reinitializes  the  system. 

Loading  "joint,  instance. cl"  creates  the  instance  of  a  joint  used  for  model  testing. 
The  functions  provided  allow  various  orders  to  be  sent  to  the  joint  and  then  displays 
system  response  to  those  orders.  "Move-joint-mult"  orders  multiple  repetitions  of  the 
same  "delta-theta"  order,  each  order  being  initiated  upon  completion  of  the  previous 
order.  "Move-joint-list"  is  similar  in  execution,  but  takes  a  list  (sequence)  of  delta-theta's 
rather  than  repeating  the  same  one.  "Run-joint"  orders  a  continuous  sequence  of  small 
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delta-thetas,  determined  by  argument  "speed"  and  system  elapsed  time,  required  to  achieve 
the  ordered  RPM  speed.  The  remaining  functions  are  intended  to  be  internal  calls.  They 
are  "clear-and-reset"  (called  by  each  of  the  previous  three  for  initialization), 
"move-joint- list- 2"  (recursive  call  for  move-joint-list),  and  "move-joint"  (called  by 
move-joint-mult  and  move-joint-list-2).  "Move-joint"  is  the  workhorse  and  makes 
repeated  calls  to  method  "increment-joint"  until  the  ordered  delta-theta  has  been  achieved 
(the  pulse  counter's  current  count  approaches  zero).  In  contrast,  "run-joint"  makes 
repeated  calls  to  "increment-joint"  until  the  ordered  speed  is  achieved. 

3.  Additional  Supporting  Code 

Loading  "Window. instanced"  creates  a  display  window  with  a  gradicule.  A  call 
to  "display- st ate"  reads  the  appropriate  state  slots  and  draws  the  set  of  data  points  for  the 
current  time.  Additional  methods  are  provided  for  internal  calls  and  for  reinitialization 
(i.e.  clearing  or  resetting  the  window).  The  end  result  is  a  display  of  system  state  vs.  time 
for  program  output.  Finally,  the  file  "joint-loader"  is  provided  as  a  convenience,  and 
loading  it  ensures  loading  of  the  source  code  files  in  the  correct  sequence  (dependencies 
are  observed).  After  loading  the  source  code,  it  then  makes  calls  that  test  run  various 
features  of  the  system. 

E.   SUMMARY 

This   chapter   provides    a    review   of  the   basic    mathematics    required   to    model 
servomotors  and  reduction  gears  and  then  describes  the  simulation  of  an  Aquarobot  joint 
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actuator.  The  model  is  simplified  in  that  exception  handling  control  signals  are  ignored 
and  the  pulse  counter  input  is  desired  delta-theta  for  the  joint  shaft,  but  functionally,  it  is 
equivalent.  In  the  next  chapter,  the  Aquarobot  model  is  presented;  however,  joint 
actuators  are  not  yet  included  but  instead  are  functionally  represented  by  springs  and 
dampers. 
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V.    SPRING  AND  DAMPER  MODEL 

A.   INTRODUCTION 

A  modified  dynamic  simulation  model  for  Aquarobot  is  developed  in  this  chapter  It 
has  springs  and  dampers  in  place  of  servo  motors  to  provide  joint  torques.  The  springs 
and  dampers  are  intended  to  eventually  be  replaced  by  a  joint  actuator  model  such  as  that 
presented  in  the  previous  chapter.  The  model  can  be  tested  by  giving  the  Aquarobot  an 
initial  position  and  orientation  and  then  allowing  it  to  drop,  observing  the  response  as  the 
feet  contact  the  ground  and  the  legs  provide  support. 

Since  the  purpose  of  the  model  is  to  provide  an  approximate  dynamic  simulation, 
capable  of  running  in  real  time,  several  simplifications  have  been  made: 

( 1 )  the  legs  are  considered  to  be  massless; 

(2)  the  ground- foot  friction  is  infinite  (no  slippage); 

(3)  the  center  of  mass  is  assumed  to  be  at  the  geometric 
center  of  the  inboard  joints  of  the  legs;  and 

(4)  body  inertia  is  that  of  a  solid  homogeneous  cylinder. 

In  addition,  joint  stops  (physical  limits)  in  the  kinematic  model  are  disabled  and  collisions. 
other  than  feet  contacting  the  ground,  have  been  ignored. 
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B.   INVERSE  KINEMATICS 

The  Aquarobot  leg  kinematic  model  allows  determination  of  foot  position,  given  the 
joint  angles.  The  inverse  kinematic  problem  on  the  other  hand,  is  to  determine  the  joint 
angles,  given  the  foot  position.  Using  the  appropriate  coordinate  systems  simplifies 
inverse  kinematics.  Figure  5.1  illustrates  the  coordinate  system  used  to  calculate  joint  l's 
angle,  theta  1:  the  joint  is  the  origin,  the  Z-axis  is  down  (parallel  to  body  Z-axis)  and  the 
X-axis  is  radially  outward  from  the  body.  Theta  1  is  measured  as  a  right  handed  Z-axis 
rotation  using  the  X-axis  as  a  zero  reference. 
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Figure  5.1 
Top  View  of  Joint  1  Coordinate  System  Used  to  Calculate  Theta  1. 


Given  the  foot  position  in  this  coordinate  system,  theta  1  is  easily  calculated: 


0 1  =  arctan  [  -£r~ 


(5.1) 
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Figure  5.2 
Side  View  of  Coordinate  Svstem  Used  to  Calculate  Theta2  and  Theta3. 


Figure  5.2  illustrates  the  coordinate  system  used  for  calculating  theta2  and  theta3.  Joint2 
serves  as  the  origin,  the  X-axis  is  defined  as  the  direction  directly  away  from  joint  1.  and 
the  Z-axis  is  again  down.  Using  this  coordinate  system  reduces  the  problem  to  two 
dimensions  as  the  y-component  of  the  foot  position  is  now  zero  Figure  5.3  and  Equation 
5.2  illustrate  the  law  of  cosines  [Oakley,  1971]  which  is  used  to  determine  theta2  and 
theta3. 
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Figure  5.3 
Applying  Law  of  Cosines 
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Solving  Equation  5.2  for  angle  A: 


A  =  arccos 


b-  <rc--cr 
Ibc 


(5.3) 


Angles  B  and  C  are  determined  in  the  same  manner: 


B  =  arccos 

<r  +t  z-b2 
Ziic 

C  -  arccos 

a:+b2-c2 
lab 

(5.4) 


(5.5) 


Referring  back  to  Figure  5.3,  sides  a  and  b  correspond  directly  to  the  lengths  of  link2  and 
link3,  respectively,  and  side  c  may  be  calculated  using  Pythagorean's  theorem: 


c=  Jx}00t+z}00l  .  (5.6) 

Angle  D  may  be  determined  by  several  trigonomic  functions;  arcsin  is  used  here: 

D  =  arcsin  [^].  (5.7) 

Theta2  and  theta3  are  measured  using  the  sign  convention  shown.   Note  that  theta3  is  the 
negative  of  C's  compliment: 

02=5-D,  (5.8) 

and 

e3  =  C-7i.  (5.9) 

Combining  Equations,  (5.4,  5.7  and  5.8)  and  (5.5  and  5.9),  and  substituting  L2  and  L3  for 
a  and  b  results  in 


0?  =  arccos 


03  =  arccos 


Ll+c2-L] 
ILiC 


L\+L\-cl 


ALlL 


-arcsin 


-n. 


[>],and 


(5.10) 
(5.11) 
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C.  JACOBIAN  MATRIX 


The  Jacobian  Matrix.  Jr(q)  or  simply  7,  of  vector  r  with  respect  to  vector  q  is 
defined: 


M<J)  = 


(5.12) 


and  is  used  to  express  the  relation  between  an  end  effector  velocity  and  the  joint  velocities 
of  a  manipulator  [Yoshikavva.  1990].  In  the  case  of  Aquarobot.  the  foot  velocity  with 
respect  to  leg  joint  velocities  is  given  by: 


1  foot 
[  tool 
'fool 


-  [J foot  ]-,.-, 


9, 
92 

e3 


(5.13) 


By  rearranging  Equation  5. 13.  the  inverse  Jacobian  and  the  foot  velocity  may  be  used  to 
determine  the  joint  velocities: 


e, 
e3 


V> 


toot] 


Xfoot 

y foot 

-foot 


(5.14) 


The  Jacobian  will  also  be  used  in  the  next  section  to  determine  ground  reaction  forces 

The  derivation  of  the  Jacobian  matrix  for  an  Aquarobot  leg  is  straightforward.  The 
foot  position  of  an  Aquarobot  leg  is  kinematically  described  as  a  function  of  the  joint 
angles  in  [Schue,  1993]: 

x  =  IoCOsGo  +Licos6oi  +Z,2cos9()icos0:  +Z,icos0mcos62 ,  (5. 15) 


y  -  LosinOo  +Lisin0oi  +L:sin0oicos02  +Z,3sin0oicos923, 
r  =  -  Z,2sin02  -Z^sin©?;?, 


(5.16) 
(5.17) 
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where  L  is  link  lenath,  theta  is  joint  angle,  theta,,  is  the  sum  of  theta  and  theta.  and  link 
represents  the  constant  pseudo-link  from  center  of  body  to  joint,  and  has  a  joint  angle 
equal  to  the  "leg  attachment  angle".   Differentiating  Equations  5. 15  through  5. 17  gives: 

x  =  -L\[ sinOoi  0i  )  -£:(  sin0oicos02  0i  +  cos0oisin02  02 


£3  I  Sin  001  COS  023  01     +COS0o]Sin023l  02  +03 


•  • 


(5.18) 


y  =  -LA  cos0oi  0]  j  -£2 1  cos 0oi cos 02  0]  +sin0oisin0;  0: 


-L3I  cos0nicos023  0i   +  sin 0r,i sin 023 1  0:  +03 


•  • 


Li    COS02  02        -Z<3    COS023    02  +03 


•  • 


(519) 
(5.20) 


Regrouping  and  translating  Equations  5.18  through  5.20  into  the  form  of  Equation  5.13, 
the  Jacobian  Matrix  is  given  by 


J= 


-(L\  +  L2COSQ2  +Z.3Cos823)sin0Oi   -(Z-2Sin92  -Z,3sin923)cos6oi   -/.3cos9oisin923 
(L\  +  Licos&i  +  £3cos823)cos9oi    -(Z-2Sin92  -Z,3Sin923)sin9oi    -Z.3sin9oisin923 

0  -Z-2COS92  -Z,3COS923  -Z,3COS923 


(5.21) 


D.   FORCES  ON  AQUAROBOT 

Assuming  homogeneous  cylindrical  distribution  of  body  mass  and  massless  legs 
reduces  the  complexity  of  the  forces  and  torques  on  Aquarobot.  As  Figure  5.4  illustrates, 
the  resulting  summations  of  these  forces  and  torques  may  be  expressed 


/ 


Aquarobot 


=  mg+  I     fieg, 
leg=\ 


(5.22) 
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and 


'   Aquarobot  ~    —     ' /f,?  *  J  leg 
leg=\ 


(5.23) 


where  ^    is  ground  reaction  force,  and  r.    is  the  moment  arm.  from  the  body's  center  of 
mass  to  the  foot,  on  which  that  force  is  exerted. 


ground  level 


foot  in  contact 


Figure  5.4 
Forces  on  Aquarobot 

Given  an  initial  known  state  (position,  orientation  and  velocity),  Aquarobot's  motion  may 
be  completely  described  by  application  of  Newton's  second  law.  The  kinematic  solution 
for  ru  is  already  available  [Davidson,  1993],  so  all  that  remains  is  determination  of^  . 

The  torques  at  the  joints  in  a  manipulator  are  related  to  the  force  exerted  by  the  end 
effector  by  the  transpose  of  the  Jacobian  [Yoshikawa,  1990]: 


T  = 


*e3 


=  [JYf 


(5.24) 


37 


where/ is  the  force  the  foot  exerts  on  the  ground,  equal  magnitude  but  opposite  direction 
of  ground  reaction  force.  To  avoid  confusion.  /  will  thus  refer  to  the  ground  reaction 
force,  the  force  the  gTound  exerts  on  the  foot,  as  it  is  the  force  in  which  we  are  interested; 
therefore  Equation  5.24  must  have  a  sign  change: 


T=[J]T[-~f) 


Solving  Equation  5.25  for/  yields: 


f  =-VT\     T- 


(5.25) 


(5.26) 


E.   SPRING  AND  DAMPER  JOINT  TORQUES 

In  the  spring  and  damper  model,  joint  torque  is  the  sum  of  spring  restoring  torque  and 
damping  torque: 

(5.27) 


TyoiWr=(-^(e-e0))  +  ^ej, 

T  Vwg 

* damp 

where  ks  and  kA  are  spring  and  damping  constants,  and  thetaO  is  the  ordered  position.  For 
the  remainder  of  this  chapter,  thetaO  will  be  taken  as  the  rest  position  or  zero  torque 
reference.  For  convenience,  the  symbols  on  the  right  side  of  Equation  5.27  will  be  used  to 
refer  to  their  vector  forms  and  will  represent  all  three  joints  in  a  leg.  Replacing  the  left 
side  with  T,  and  assuming  the  same  spring  and  damping  constants  for  each  joint, 


(5.28) 
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The  joint  angles  are  determined  using  the  inverse  kinematic  method  described  abo\  e 
and  are  then  available  for  the  Jacobian  matrix.  For  any  foot  in  contact  with  the  ground,  its 
velocity,  relative  to  the  body,  is  simply  the  negative  of  the  sum  of  the  body's  translational 
velocity  and  the  cross  product  of  the  body's  rotational  velocity  with  the  foot's  position 
vector.  Combining  Equations  5.14  and  5.27,  and  substituting  for  foot  velocity  gives  us: 

T=  -ks  (9-e0)+  kdJ~]  [vWv  +(ioho(hxr,ool)l  (5.29) 

which  is  now  substituted  into  Equation  5 .26  to  give  us  the  ground  reaction  force: 

/  =  [Jl]    [ks(Q-Qo)-kdJ-l[vbod)  +(abodyxrfoot)]  .  (5.30) 

F.    LISP  PROTOTYPE 

The  kinematic  model  for  the  Aquarobot  simulation  was  borrowed  from  [Davidson. 
1993]  with  the  only  code  modification  being  the  conversion  to  Modified 
Dan evit- Hart enberg  (MDH)  coordinate  systems  to  match  the  C++  version.  The  complete 
source  code  listing  with  loading  and  operating  instructions  may  be  found  in  Appendix  C. 
The  additions  required  for  dynamic  operation  of  the  model  are  best  presented  by  the 
following  walk-through  of  a  dynamic  update.  Figure  5.5  provides  a  flowchart  for 
reference. 

A  dynamic  update  of  Aquarobot  is  achieved  by  calling  method  "update-aquarobot". 
The  body's  acceleration,  velocity  and  position  are  all  updated  by  calls  to  previously  defined 
methods  in  the  rigid-body  class.  After  these  are  completed,  and  the  body  is  repositioned, 
the  legs,  forces  and  torques  are  then  updated  by  calling  "update- forces-and-torques" 
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Figure  5.5 
Flowchart  for  Dynamic  Update  of  Aquarobot. 


Gravity  is  handled  separately  in  the  "update-acceleration"  method;  therefore,  only  the 
forces  and  torques  due  to  foot  contact  with  the  ground  need  be  considered. 
Update-forces-and-torques  resets  the  body's  forces- and-torques  vector,  a  rigid-body  slot, 
to  zeros,  then  calls  "add-leg-forces-and-torques"  for  each  leg  to  generate  an  updated 
cumulative  value. 
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Add-leg-forces-and-torques  tests  for  "foot-contact",  a  Boolean  type  slot,  or 
"new-contact",  a  function  that  sets  foot-contact  to  true  and  the  foot's  world  z  coordinate 
to  zero  (ground  level)  if  ground  penetration  is  detected  (z  coordinate  greater  than  zero). 
If  there  is  no  foot  contact,  nothing  happens:  joint  angles  remain  set  to  their  default  values 
and  the  body's  cumulative  forces-and-torques  value  is  left  unchanged.  If  however,  there  is 
contact,  the  inverse  kinematics  routine  is  called,  the  ground  reaction  force  is  calculated 
using  Equation  5.30,  and  the  joint  angles  are  updated.  Before  updating  the  cumulative 
forces-and-torques,  loss  of  contact  must  be  detected.  This  is  done  by  testing  the  world  z 
component  of  the  calculated  ground  reaction  force  in  a  call  to  "still-in-contact".  If  the 
force  is  such  that  it  is  pulling  the  robot  down  rather  than  supporting  it.  then  foot-contact  is 
set  to  false,  the  joint  angles  are  reset  to  their  default  values,  and  again,  no  contribution  to 
forces-and-torques.  If  the  foot  is  determined  to  be  still-in-contact,  world  z  component  of 
the  force  less  than  zero  (pushing  up),  then  method  "add- forces- and-torques-to-body"  is 
called  which  adds  /  and  r  x  /  to  the  cumulative  value  of  forces-and-torques  as  in 
Equations  5.22  and  5.23.  After  cycling  through  all  the  legs,  Aquarobot  is  completely 
updated  and  ready  for  another  cycle. 

This  dynamic  update  cycle  actually  uses  the  (/  +l)lh  velocity  for  the  ilh  update.  While 
the  ilh  velocity  might  just  as  easily  have  been  used,  it  is  neither  better  nor  worse.  Better  is 
actually  the  average  of  the  two. 
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G.  C++ PROTOTYPE 

The  0+  version  of  the  dynamic  Aquarobot  model  is  algorithmically  identical  to  the 
LISP  version.  A  complete  source  code  listing  with  operating  instructions  may  be  found  in 
Appendix  D.  As  discussed  in  Chapter  III.  use  of  IRIS  Performer  structures  required  some 
modifications.  For  example,  six-vectors  were  divided  into  pairs  of  three- vectors.  The 
dynamic  model  is  otherwise  identical.  A  feature  added  to  this  version  is  the  ability  to  pass 
spring  and  damper  constants,  drop  height,  and  dynamic  update  time  increment  into  main 
as  command  line  arguments.  After  handling  these  optional  arguments,  main  initializes 
Performer,  instantiates  and  initializes  dynamic  and  graphic  Aquarobot  objects,  then  cycles 
in  an  update-render  loop. 

The  "graphic  Aquarobot"  object  was  not  required  in  the  LISP  version  as  the  "dynamic 
Aquarobot"  slot  values  were  directly  accessed  to  render  a  stick  figure.  This  version, 
however,  draws  thousands  of  filled  polygons  each  cycle  to  render  a  single  frame.  IRIS 
Performer  was  used,  as  previously  stated,  primarily  for  its  high  performance  in  this  task. 
The  graphic  Aquarobot  is  a  hierarchical  database  containing  the  information  required  to 
draw  Aquarobot.  After  each  dynamic  update,  the  body's  position  and  orientation  and  the 
leg's  joint  angles  are  passed  into  "Dynamic  Coordinate  Systems"  in  the  database  prior  to 
calling  the  draw  routine. 
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H.  SUMMARY 

This  chapter  develops  a  simplified  dynamic  simulation  model  for  Aquarobot  using 
springs  and  dampers  to  provide  the  joint  torques.  Inverse  kinematics.  Jacobian  matrices 
and  their  utility,  and  forces  acting  on  Aquarobot  are  discussed  The  model  was 
implemented  and  tested  using  both  LISP  and  C++.  In  the  next  chapter,  simulation  results 
of  the  spring  and  damper  model,  and  also  of  the  joint  actuator  model  from  Chapter  IV.  are 
presented. 
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VI.    SIMULATION  RESULTS 

A.  INTRODUCTION 

This  chapter  presents  the  simulation  results.  The  joint  actuator  simulation  model  was 
tested  to  verify  the  model  and  to  experimentally  determine  suitable  amplifier  gains.  As 
stated  in  the  previous  chapter,  the  spring  and  damper  Aquarobot  model  was  tested  by 
dropping  the  robot  from  a  low  height  and  observing  is  dynamic  behavior. 

B.  JOINT  ACTUATOR  SIMULATION 

The  motor  class  is  designed  such  that  constructor  arguments  are  parameters  listed  on 
the  motor  specification  sheet.  At  the  time  of  the  simulation,  specification  sheets  for  the 
motors  used  in  Aquarobot  were  not  available;  therefore,  another  model  was  used.  The 
model  was  tested  by  applying  rated  voltage  to  the  motor  and  observing  its  acceleration 
and  attained  RPM  In  Figure  6. 1,  both  motor  and  output  shaft,  after  200: 1  reduction  gear, 
speeds  and  positions  are  shown.  Motor  scales  are  on  the  left,  while  output  shaft  scales  are 
on  the  right.  Qualitatively,  the  motor  model  is  well  behaved,  and  quantitatively,  it  closely 
matches  the  no-load  speed  parameter  listed  in  the  specifications. 
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joint  actuator  simulation 


Figure  6. 1 
No  Load  Joint  Actuator  Response  With  +75VDC  Applied  to  the  Motor 


To  obtain  fast  joint  actuator  response  to  input  orders,  it  is  desirable  to  set  the  error 
signal,  D/A  converter  output,  gain  to  a  relatively  high  value.  A  gain  of  150  resulted  in  the 
optimum  response.  Values  from  100  to  200  gave  satisfactory  results  while  higher  values 
progressively  reduced  the  effectiveness  of  the  degenerative  feedback.  Figures  6.2  through 
6.5  show  joint  actuator  responses  to  the  following  sequence  of  shaft  positioning  orders,  in 
revolutions,  with  various  driver  gain  values:  (+1/4,  +1/2,  -1,  -1/2,  +1,  +3).  Figure  6.2 
illustrates  the  response  with  an  error  signal  gain  of  150  and  with  velocity  and  current 
feedback  signals  disabled. 
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joint  actuator  simulation 


Figure  6.2 

Joint  Actuator  Response  with  Error  Signal  Gain  of  150. 

Velocity  and  Current  Feedback  Disabled. 


The  optimum  velocity  feedback  signal  gain  is  a  compromise  between  response  time 
and  stability.  Too  high  a  value  results  in  slower  response,  while  lower  values  allow 
increased  overshoot.  Values  from  three  to  seven  were  satisfactory.  A  gain  of  five  proved 
optimum  when  combined  with  current  feedback.  Figure  6.3  displays  the  results  of  using 
various  gain  values  for  velocity  feedback  with  current  feedback  still  disabled. 
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Figure  6.3a 
Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  3 
and  Error  Signal  Gain  of  150.   Current  Feedback  Disabled 


joint  actuator  simulation 


Figure  6.3b 
Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  5 
and  Error  Signal  Gain  of  150    Current  Feedback  Disabled 
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Figure  6.3c 
Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  7 
and  Error  Signal  Gain  of  150.   Current  Feedback  Disabled. 


joint  actuator  simulation 


Figure  6.3d 

Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  20 

and  Error  Signal  Gain  of  150.  Current  Feedback  Disabled. 
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The  current  feedback  gain  value  turned  out  to  be  the  most  sensitive  A  value  of  0  3 
produced  negligible  change,  while  0.7  resulted  in  some  oscillation  This  regenerative- 
feedback,  intended  to  reduce  response  time,  also  aided  in  reducing  the  overshoot  As  the 
error  signal  approached  zero  with  speed  on,  back  EMF  caused  a  decelerating  current 
which  was  aided  by  the  regenerative  current  feedback  Figure  6  4  displays  the  results  of 
using  various  gain  values  for  current  feedback  with  velocity  feedback  again  disabled  In 
addition,  in  the  presence  of  the  degenerative  velocity  feedback,  the  error  signal  was 
overcome  earlier  by  decelerating  signals,  bringing  the  current  feedback  contribution  in 
even  sooner.  Figure  6.5  illustrates  results  with  both  velocity  and  current  feedbacks  in 
effect.  The  response  of  the  joint  actuator  in  this  simulation  highlights  the  effectiveness  of 
the  design:  high  error  signal  gain  with  velocity  and  current  compensating  feedback 
networks 

While  these  tests  results  are  not  for  the  specific  motors  used  in  Aquarobot,  they 
provide  a  good  set  of  gain  parameters  to  use  as  a  starting  point  in  the  experimental 
determination  of  the  gains  for  those  motors. 
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Figure  6.4a 
Joint  Actuator  Response  with  Current  Feedback  Gain  of  0.3 
and  Error  Signal  Gain  of  150.  Velocity  Feedback  Disabled. 


joint  actuator  simulation 


Figure  6.4b 
Joint  Actuator  Response  with  Current  Feedback  Gain  of  0.5 
and  Error  Signal  Gain  of  150.  Velocity  Feedback  Disabled. 
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Figure  6.4c 
Joint  Actuator  Response  with  Current  Feedback  Gain  of  1.0 
and  Error  Signal  Gain  of  150.  Velocity  Feedback  Disabled. 
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joint  actuator  simulation 


Figure  6.5a 
Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  3, 
Error  Signal  Gain  of  150,  and  Current  Feedback  Gain  of  0.5. 
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Figure  6.5b 
Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  5, 
Error  Signal  Gain  of  150,  and  Current  Feedback  Gain  of  0.5. 
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Figure  6.5c 
Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  7, 
Error  Signal  Gain  of  150,  and  Current  Feedback  Gain  of  0.5. 


juiiit  actuator  simulation 


Figure  6.5d 
Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  10, 
Error  Signal  Gain  of  150,  and  Current  Feedback  Gain  of  0.5. 
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C.  AQUAROBOT  SPRING  AND  DAMPER  SIMULATION 

LISP  and  C++  versions  of  the  spring  and  damper  Aquarobot  model  were  tested  by 
using  a  "droptest"  in  which  the  model  is  dropped  from  low  height.  It  may  be  tilted,  but 
not  so  much  that  it  does  not  land  on  its  feet.  The  LISP  version  served  as  the  prototype, 
and  after  successful  testing,  the  model  was  translated  into  C++. 

The  LISP  simulation  ran  uncompiled  on  a  Sun  Sparc- 10  at  six  to  eight  frames  per 
minute  and  was  too  slow  for  comprehension  of  motion  detail.  It  did,  however,  allow 
experimental  determination  of  spring  and  damper  constants  sufficient  to  support  the  model 
when  dropped.  To  increase  the  simulation  speed,  two  dynamic  update  cycles  were  run 
between  each  display,  and  the  source  code  was  then  compiled.  After  compilation,  the 
simulation  ran  at  near  30  frames  per  minute,  with  60  dynamic  updates  of  50ms  each,  to 
achieve  a  simulation  with  approximately  a  10:1  time  dilation.  This  simulation  was  fast 
enough  for  an  observer  to  assimilate  the  dynamic  behavior  of  the  model  which  was 
qualitatively  satisfactory  and  considered  successful.  Some  additional  fine  tuning  of 
experimentally  determined  parameters  was  done  prior  to  translation  to  C++. 

After  translation  to  C++/Performer,  the  model  was  again  tested,  and  a  real-time 
simulation  was  achieved  on  a  four  processor  IRIS  440/RE  workstation.  Only  three 
processors  were  actually  utilized,  one  assigned  to  each  of  the  following  tasks:  application, 
database  pre-draw  cull,  and  database  rendering  traversal.  The  application  processor 
utilization  was  approximately  fifteen  percent  which  indicates  that  the  increased  complexity 
of  adding  the  joint  actuators  will  not  present  any  difficulty.   The  cull  processor  utilization 
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was  approximately  twenty  percent  while  that  of  the  rendering  processor  veried  from  fiftv 
to  seventy  percent  (Note:  the  source  code  was  not  compiled  and/or  linked  with 
optimizations  on  )  Typical  images  obtained  can  be  found  in  [Goetz,  1994] 

Running  in  the  three  processor  configuration  described  above,  Performer 
synchronized  the  framerate  to  the  fixed  50ms  dynamic  updates  by  limiting  the  framerate  to 
20Hz.  On  a  single  processor  IRIS  R-4000,  where  the  application,  database  cull,  and 
database  rendering  were  forced  to  run  sequentially,  the  highest  framerate  achieved  was 
10Hz.   This  resulted  in  a  2: 1  time  dilation  (slow  down)  simulation. 

D.   SUMMARY 

Testing  of  both  simulation  models  was  considered  successful.  While  it  is  unfortunate 
that  there  was  insufficient  time  to  incorporate  the  joint  actuator  model  into  the  Aquarobot 
model,  replacing  the  springs  and  dampers,  the  topic  was  given  some  time  and  effort.  This 
next  step  is  among  the  topics  addressed  in  the  final  chapter:  Summary  and  Conclusions 
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VII.    SUMMARY  AND  CONCLUSIONS 

A.  UTILITY  OF  LISP  FOR  EXPERIMENTAL  PROGRAMMING 

The  primary  benefit  of  using  LISP  as  a  prototyping  language  in  this  thesis  was  the 
immediate  testing  capability  it  provided.  No  test  routines  were  required.  Each  function 
was  easily  tested  by  direct  calls  as  it  was  developed.  While  compilation  capability  allowed 
a  faster  simulation,  repeated  compilations  were  not  required  as  the  routines  could  called 
from  the  interpreter's  command  line.  Finally,  nesting  functions  allowed  larger  and  larger 
integrated  blocks  of  source  code  to  be  tested. 

One  of  the  benefits  of  using  LISP  during  prototyping  was  realized  when  an  apparent 
limit  cycle  appeared  if  Aquarobot  was  tilted  when  dropped.  The  problem  was  actually  a 
lack  of  rotational  damping  due  to  the  absence  of  the  omega  cross  r  term  in  the  foot 
velocity  calculation.  Without  it,  damping  ceased  shortly  after  landing  because  of  "zero 
translational  foot  velocity."  While  the  author  took  a  considerable  period  of  time  to  find 
the  cause  of  the  problem,  with  a  simple  modification  to  the  LISP  source  code,  the 
correction  was  quickly  and  easily  verified. 

B.  INCORPORATING  THE  JOINT  ACTUATOR  MODEL 

The  next  step  required  for  the  Aquarobot  dynamic  simulation  model  is  to  replace  the 
springs  and  dampers  with  joint  actuators.   This  was  initially  assumed  to  be  a  simpler  task 
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than  it  turned  out  to  be.  The  difficulty  arises  from  a  conflict  over  control  of  joint  state 
variables  In  the  spring  and  damper  model,  the  joints  only  supply  state  dependent  torques 
which  are  then  used  to  dynamically  update  the  robot's  body  After  the  body  is  updated, 
the  new  states  of  the  joints  depend  only  on  the  new  body  position  and  not  on  the  joint 
torques  In  the  joint  actuator  model,  the  joint  state  depends  directly  upon  the  motor 
torque.  Two  possible  solutions  are  proposed  to  eliminate  the  conflict. 

One  possible  solution  is  to  extend  the  massless  leg  simplification  to  the  motor  and 
gear-train,  making  them  inertialess  While  this  seems  easiest  and  will  probably  have  as 
little  impact  as  the  massless  legs,  the  overall  effect  may  be  greater  than  anticipated  due  to 
the  large  reduction  ratios  involved.  Recall  that  motor  inertia  reflected  outside  the 
reduction  gear  is  multiplied  by  the  square  of  the  reduction  ratio.  A  C-h-  version  of  the 
joint  actuator  model  is  included  in  Appendix  B.  This  model  is  a  variation  of  the  original 
LISP  version  and  provides  state  dependent  torques  as  output  rather  than  the  state  itself.  It 
could  be  used  to  implement  inertialess  joint  actuators,  and  the  changes  required  in  the 
Aquarobot  model  would  be  minor. 

Another  possible  solution  is  to  use  the  concept  of  "added  mass,"  an  apparent  increase 
in  mass  (affecting  acceleration)  due  to  the  internal  inertia  of  the  drive  motors.  Assuming  a 
unit  acceleration  for  one  of  the  body's  six  degrees  of  freedom,  it  is  possible  to  calculate  the 
resulting  joint  accelerations.  If  a  joint  acceleration  is  known,  inertial  torque  in  the  joint 
can  be  calculated.  The  net  torque  for  the  joint  then  is  armature  torque  (determined  using 
motor  applied  voltage  and  speed)  minus  the  inertial  torque.   Doing  this  for  all  six  degrees 
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of  freedom  gives  a  matrix  of  added  mass  which  can  be  inverted  to  get  acceleration  for  any 
given  vector  of  joint  motor  applied  voltages.  An  "equilibrium  torque,"  torque  vector 
which  results  in  a  zero  joint  acceleration  vector,  is  also  needed  and  must  be  calculated 
[Koozekanani,  1983]. 

C.   SUMMARY 

IRIS  Performer  has  proven  its  utility  as  a  graphics  rendering  tool  in  a  real  time 
simulation.  It  also  provided  easy  synchronization  for  fixed  duration  integration  intervals. 

A  real-time  dynamic  simulation  of  Aquarobot  was  accomplished  and  is  eventually 
expected  to  provide  a  valuable  tool  for  Aquarobot  control  software  developers.  While  an 
integrated  model,  with  the  joint  actuators  in  place,  is  not  yet  completed,  we  are  a  step 
closer,  and  the  task  has  certainly  proven  to  be  feasible.  Once  the  joint  actuators  are 
installed  into  the  Aquarobot  model,  simple  walking  simulations,  on  smooth,  flat  terrain 
may  be  achieved.  Concurrent  work  on  collision  detection  for  uneven  terrain  will  also 
further  improve  the  simulation  model  when  incorporated  [Goetz,  1994]. 

Further  work  could  improve  on  the  Performer  Aquarobot  rendering  database  to 
decrease  the  rendering  time.  This  is  the  area  where  there  is  the  most  room  for 
improvement  in  cycle  time.  Finally,  other  than  using  the  "faster"  Performer  routines,  no 
attempt  has  been  made  to  optimize  the  source  code  which  was  written  with  ease  of 
modification  in  mind.    Utilization  of  compile  and  link  optimizations,  and  eventually  some 
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source  code  tuning,  may  also  contribute  toward  achievement  of  a  real-time  Aquarobot 
simulation  on  a  single  processor 
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APPENDIX    A 


LOADING  AND  OPERATING  INSTRUCTIONS 


To  run  demo,  start  LISP  Interpreter  and  call: 
(load  "joint-loader") 


SOURCE  CODE  FILES 

;  "joint-loader" 


;  load  files 

(load  "math. routines. cl") 

(load  "time. routines. cl") 

(load  "difF-counter.class.cl") 

(load  "amplifier-clipper.class.cl") 

(load  "shaft. class. cl") 

(load  "motor. class. cl") 

(load  "reduction-gear. class. cl") 

(load  "joint. class. cl") 

(load  "joint. instanced") 

(load  "window. instanced") 

;  execute  tests 
(move-joint-mult  -.25  4) 
(move-joint-mult  .25  4) 
(move-joint-mult  .05  4) 
(move-joint-list  '(25  .5  -1  -.5  1  3)) 
(run-joint  15) 
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;  "diff-counter.class.cl" 

(defclass  diff-counter  () 
((current-count 

accessor  current-count 
mitforni  0 
:type       float  ))) 

(defmethod  diff-signal  ((dc  diff-counter)  plus-input  minus-input) 
(setf  (current-count  dc) 

(-1-  (current-count  dc)  plus-input  (-  minus-input)))) 

;  "amplifier-clipper.class.cl" 


(defclass  amplifier-clipper  () 
((amplification-factor 

mitarg    amplification-factor 
accessor  amplification-factor 
lmtform   1 
type       float) 
(max-value 
:initarg  :  max-value 
accessor  max-value 
lmtform   1 
type       float) 
(nun-value 
:initarg    nun-value 
accessor  min-value 
:initform  -1 
:type       float))) 

(defmethod  amplify  ((amp  amplifier-clipper)  input-value) 
(max  (min  (*  (amplification-factor  amp)  input-value) 
(max-value  amp)) 
(min-value  amp))) 
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(defclass  motor-driver  (amplifier-clipper) 
((displacement-gain 

imtarg  :displacement-gain 
accessor  displacement-gain 
initform   1 
:type       float) 
(velocity -fb-gain 
imtarg    velocity -fb-gain 
accessor  velocity -fb-gain 
: initform  0 
:type       float) 
(current-fb-gain 
imtarg    current-fb-gain 
accessor  current-fb-gain 
initform  0 
:type       float))) 

(defmethod  dnve((dnver  motor-dnver)  displacement-input 
velocity-input 
current-input) 
(amplify  driver  (+  (*  displacement-input  (displacement-gain  driver)) 
(*  (-  velocity  -input)  (velocity -fb-gain  driver)) 
(*  current-input       (curTent-fb-gain    driver))))) 
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;  "shaft.class.cl" 


(defclass  shaft  () 

((angular-position  ;  radians 

accessor  theta 
mitform  0 
t\pe       float) 
(angular-velocity  .  rad/sec 

accessor  omega 
mitform  0 
type       float) 
(inertia  .  Kg-(  meters-square) 

initarg    I 
accessor  I 
mitform  0 
:t>pe       float) 
(coulomb-fnction-constant  ;  Newton-meters  (Fc  >=  0) 
initarg    Fc 
accessor  Fc 
mitform  0 
:t>pe       float) 
(viscous-friction-constant  ;  Ne\vton-meters/(  rad/sec)  (Fv  >=  0) 
initarg    Fv 
accessor  Fv 
imtform  0 
:t\pe       float) 
(time-stamp  ;  seconds 

accessor  time-stamp 
:initform  (system-time)))) 


(defmethod  set-shaft  ((s  shaft)  theta  omega) 
(setf  (omega  s)  omega) 
(setf  (theta  s)  theta)) 

(defmethod  reset-shaft  ((s  shaft)) 
(set-shaft  s  0  0) 
(setf  (time-stamp  s)  (system-time))) 

(defmethod  connect  ((source  shaft)  (load  shaft)) 
(setf  (time-stamp  load)  (time-stamp  source)) 
(set-shaft  load  (theta  source)  (omega  source))) 
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"motor.class.cl" 


(defclass  motor  (shaft) 
((torque-constant      ;  Newton-meters/ampere 

:irutarg  :Kt 

:accessor  Kt 

t\pe       float) 
(back-emf-constant    .  Volts/(rad/sec) 

:initarg    Kb 

: accessor  Kb 

:t\pe       float) 
(armature-resistance  ;  ohms  (must  be  >  0) 

mitarg    R 

accessor  R 

:imtform   1 

:type       float) 
(max-brush-drop       .  volts 

:initarg  :  max-brush-drop 

accessor  max -brush-drop 

initform  2.0 

:rype       float) 
(half-brush-drop-source-value  ;  volts 

initarg  :half-BD -value 

: accessor  half-BD -value 

initform  3.0 

type      float) 
(armature-current     ;  amperes  (saved  for  feedback  purposes) 

accessor  armature-current 

:  initform  0 

:type       float))) 

(defmethod  applied-voltage  ((m  motor)  source-voltage) 
(if  (zerop  source-voltage)  0 
(*  source-voltage 

(-1 
(*  (/  (max -brush-drop  m)  (abs  source-voltage)) 

(-1 
(exp(*(log0.5) 

(/  (abs  source-voltage) 
(half-BD-value  m)))))))))) 

(defmethod  developed-torque  ((m  motor)  source-voltage) 
(setf  (armature-current  m) 

(/  (-  (applied-voltage  m  source-voltage)  (*  (Kb  m)  (omega  m)))  (R  m))) 
(*  (Kt  m)  (armature-current  m))) 
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(defmcthod  omega-dot  ((m  motor)  source-voltage  load-inenia  load-torque 
load-coulomb-fnction-constant 
load-viscous-friction-constant ) 
(let*  ((torque  (+  (developed-torque  m  source-voltage)  load-torque)) 
(Fc-total  (-•-  (Fc  m)  load-coulomb-fnction-constant)) 
(fnction-loss 
(if  (zerop  (omega  m)) 
(if  (zerop  torque) 
0 

(if  (>  Fc-total  (abs  torque)) 
torque 

(*  Fc-total  (sgn  torque)))) 
(+  (*  (+  (Fv  m)  load-viscous-fnction-constant)  (omega  m)) 
(*  Fc-total  (sgn  (omega  m))))))) 
(/  (-  torque  fnction-loss)  (+  (I  m)  load-inertia)))) 

(defmethod  run-motor  ((m  motor)  source-voltage  load-inertia  load-torque 
load-coulomb-fnction-constant 
load-viscous-friction-constant) 
(let  ((dt  (delta-time  (time-stamp  m))) 

(omega-dot  (omega-dot  m  source-voltage  load-inertia  load-torque 
load-coulomb-fnction-constant 
load-viscous-fnction-constant))) 
(setf  (theta  m)  (+  (theta  m)  (*  (omega  m)  dt))) 
(setf  (omega  m)  (+  (omega  m)  (*  omega-dot  dt))) 
(setf  (time-stamp  m)  (+  (time-stamp  m)  dt)))) 
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;  "reduction-gear.class.cl" 


(defclass  reduction-gear  () 
((gear-ratio 
:initarg  :gear-ratio 
accessor  gear-ratio 
uniform    1 
r\pe       float) 
(in-shaft 
:initarg  :in-shaft 
accessor  in-shaft 
lnitform  (make-instance  'shaft)) 
(out-shaft 
initarg    out-shaft 
accessor  out-shaft 
initform  (make-instance  'shaft)))) 

(defmethod  rg-reduce  ((rg  reduction-gear)  value) 
(/  value  (gear-ratio  rg))) 

(defmethod  rg-reflect  ((rg  reduction-gear)  value) 
(*  value  (gear-ratio  rg))) 

(defmethod  rg-inertia-forward  ((rg  reduction-gear)  inertia-value) 
(*  inertia-value  (sqr  (gear-ratio  rg)))) 

(defmethod  rg-inertia-backvvard  ((rg  reduction-gear)  inertia-value) 
(/  inertia-value  (sqr  (gear-ratio  rg)))) 

(defmethod  rg -connect  ((rg  reduction-gear)) 
(set-shaft  (out-shaft  rg) 

(rg-reduce  rg  (theta  (in-shaft  rg))) 
(rg-reduce  rg  (omega  (in-shaft  rg))))) 
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;  "joint.class.cl" 


(defclass  joint  () 
((pulse-counter 

mitarg     pc 

accessor  pc 

tnitform  (make-instance  'diff-counter)) 
(driver 

mitarg     driver 

accessor  driver 

initform  (make-instance  'motor-driver)) 
(prime-mover 

mitarg    :pnme-mover 

accessor  prime-mover 

initform  (make-instance  'motor)) 
(red-gear 

:initarg    :  red-gear 

accessor  red-gear 

initform  (make-instance  'reduction-gear)) 
(load-shaft 
:initarg    :  load-shaft 

accessor  load-shaft 

initform  (make-instance  'shaft)))) 

(defmethod  motor-load-inertia  ((j  joint)) 
(+  (I  (in-shaft  (red-gear  j))) 
(rg-inertia-backward  (red-gear  j)  (+  (I  (out-shaft  (red-gear  j))) 
(I  (load-shaft  j)))))) 

(defmethod  motor-load-coulomb-fnction-constant  ((j  joint)) 
(+  (Fc  (in-shaft  ( red-gear  j))) 
(rg-reduce  (red-gear  j)  (+  (Fc  (out-shaft  (red-gear  j))) 
(Fc  (load-shaft  j)))))) 

(defmethod  motor-load-viscous-fnction-constant  ((j  joint)) 
(+  (Fv  (in-shaft  (red-gear  j))) 
(rg-reduce  (red-gear  j)  (+  (Fv  (out-shaft  (red-gear  j))) 
(Fv  (load-shaft  j)))))) 

(defmethod  feedback  ((j  joint)) 
(-  (theta  (out-shaft  (red-gear  j)))  (theta  ( load-shaft  j)))) 
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(defmethod  increment-joint  ({j  joint)  order) 
(run-motor  (pnme-mover  j) 

(drive  (driver j)  (diff-signal  (pcj)  order  (feedback j)) 

(*  .003  (RAD/SECtoRPM  (omega  (pnme-mover  j)))) 
(armature-current  (pnme-mover  j))) 
(motor-load-inertia  j) 
0  ;  load  not  producing  any  torque 
( motor-load-coulomb-fnction-constant  j ) 
( motor-load-viscous-fhction-constant  j ) ) 
(connect  (out-shaft  ( red-gear  j))  ( load-shaft  j ) ) 
(connect  (pnme-mover  j)  (in-shaft  (red-gear  j))) 
(rg-connect  (red-gear  j))) 

(defmethod  step-input-to-joint  ((j  joint)  step) 
(diff-signal  (pcj)  (REVtoRAD  step)  0)) 

(defmethod  reset-joint  ((j  joint)) 
(serf  (current-count  (pcj))  0) 
(reset-shaft  (load-shaft  j)) 
(reset-shaft  (out-shaft  (red-gear  j))) 
(reset-shaft  (in-shaft  ( red-gear  j))) 
(reset-shaft  (pnme-mover  j))) 
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;  "joint. instanced" 


(selfjointl  (make-instance 'joint 

:dnver       (make-instance  'motor-dmcr 

amplification-factor    1 

max-value  75    ;  volts 

min-value  -75    ;  volts 

displacement-gain      150 

velocity-fb-gain        5 

:current-fb-gain        .5  ) 
pnme-mover  (make-instance  'motor 

I  .0005   ;Kg-m*m 

Fc  0075   ;  N-m 

Fv  00004  .  N-m/(rad/sec) 

Kt  ,005    ;  N-m/ampere 

:Kb  255    ;  Volts/(rad/sec) 

:R  1     )  ;  ohms 

:red-gear     (make-instance  'reduction-gear 

:gear-ratio  200) 
:load-shaft  (make-instance 'shaft 

I  5        ;  Kg-m*m 

Fc         .1        ;N-m 

:Fv         .02  ))) ;  N-m/(rad/sec) 

(defun  move-joint  (delta-theta) 
(serf (time-stamp  (pnme-mover  joint  1))  (system-time)) 
;input  delta-theta  a  little  at  a  time 
(if  (<  delta-theta  0) 

;negative  delta-theta  (use  -0.015  steps) 
(do*  ((input-index  0  (+  input-index  0.015))) 
((<  (+  delta-theta  input-index)  0.015) 
(step-input-to-joint  joint  1  (+  delta-theta  input-index))) 
(step-input-to-joint  joint  1  -0.015) 
(increment-joint  joint  1  0) 

(display-state  *display*  joint  1  (time-stamp  (pnme-mover  joint  1)))) 
;positive  delta-theta  (use  0.015  steps) 
(do*  ((input-index  0  (+  input-index  0.015))) 
((<  (-  delta-theta  input-index)  0.015) 
(step-input-to-joint  joint  1  (-  delta-theta  input-index))) 
(step-input-to-joint  joint  1  0.015) 
(increment-joint  joint  1  0) 

(display-state  *display*  joint  1  (time-stamp  (pnme-mover  joint  1))))) 
;delta-theta  entry  into  PC  is  complete 
.cycle  until  ordered  position  is  reached 
(do*  ((index  1)) 

((and  (<  (abs  (cunent-count  (pc  joint  1)))  0.05) 

(<  (abs  (omega  (pnme-mover  joint  1)))  10))  (ppnnt  'stop)) 
(increment-joint  joint  1  0) 
(display-state  *display*  joint  1  (time-stamp  (pnme-mover  joint  1))))) 
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(defun  move-joint-mult  (delta-theta  mult) 
(clear-and-reset) 
(dotimes  (i  mult)  (move-joint  delta-theta))) 

(defun  run-joint  (speed) 
(clear-and-reset) 

(do*  ((dtime  (delta-time  (time-stamp  (prime-mover  joint  1))) 
(delta-time  (time-stamp  (prime-mover  jointl))))) 
((<  (abs  (-  (RPMtoRAD/SEC  speed)  (omega  (load-shaft  joint  1)))) 

0. 1)  (ppnnt  'stop)) 
(increment-joint  joint  1  (*  dtime  (RPMtoRAD/SEC  speed))) 
(display-state  ^display*  jointl  (time-stamp  (prime-mover  jointl))))) 

(defun  move-joint-list  (delta-theta-list) 
(clear-and-reset) 

(if  (equalp  nil  delta-theta-list)  nil 
(move-joint-list-2  delta-theta-list))) 

(defun  move-joint-list-2  (delta-theta-list) 
(move-joint  (car  delta-theta-list)) 
(if  (equalp  nil  (cdr  delta-theta-list))  nil 
(move-joint-list-2  (cdr  delta-theta-list)))) 

(defun  clear-and-reset  () 
(update-minutes  *  display*  0) 
(reset-system-time) 
(reset-joint  jointl)) 


;  "window. instanced" 

;  dimensions  for  x-y  coord  system  (window  size  auto  adjusts) 


(serf  *x-ongin* 

50) 

(setf  *x-length* 

500) 

(setf  *x-tics* 

6) 

(setf  *y-ongin* 

50) 

(setf  *y-length* 

340) 

(setf  *y-tics* 

8) 

(setf  *max-speed*  4000) ;  (max  rpm's  of  motor  scale) 
(setf  *max-revs*    1000) ;  (max  rev's  of  motor  scale) 

(require  :xcw) 

(use-package  :cw) 

(cw:initialize-common-windows) 
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(dcfmethod  draw-grid  ((window  window-stream)) 
(draw-line-xy  window  *x-origin*  .top  border 

(+  *y -origin*  *\ -length*) 
(+  *\-ongin*  *x-length*) 
(+  *\ -origin*  *\ -length*)) 
(draw-line-xy  window  *x-ongin*  ;middle  \  axis 

(+  *y-ongin*  (/  *\ -length*  2)) 
(+  *\-ongin*  *x-length*) 
(+  *y-ongin*  (/  *y-length*  2))) 
(draw-line-xy  window  *\-ongin*  ;bottom  border 

*> -origin* 

(+  *\-ongin*  *\-length*) 
*y-ongin*) 
(draw-line-xy  window  *x-origin*  lett  border 

*\-ongin* 
*\-ongin* 

(+  *y-ongin*  *y-length*)) 
(draw-line-xy  window  (+  *.\-ongin*  *.\-length*)         :nght  border 
*y-ongin* 

(+  *\-ongin*  *x-length*) 
(+  *y-ongin*  *y-length*)) 
(dotimes  (i  *x-tics*)  ;  mark  x  axis 

(draw-line-xy  window  (+  *x-ongin* 

(/(*  1  *x-length*)  *x-tics*)) 
*y-ongin* 
(+  *x-ongin* 

(/(*  i  *x-length*)  *x-tics*)) 
(+  *y-ongin*  *y-length*) 
:dashing'(l  3))) 
(dotimes  (i  *y-tics*)  .  mark  y  axis 

(draw-line-xy  window  *x-ongin* 
(+  *y-ongin* 

(/  (*  l  *y-length*)  *y-tics*)) 
(+  *x-ongin*  *x-length*) 
(+  *y-ongin* 

(/  (*  l  *y-length*)  *y-tics*)) 
:dashing'(l  3))) 
(label-graph  window)) 
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(defmethod  label-graph  ((w  window-stream)) 
;  theta  labels 

(serf (window-stream-y-position  w)  (+  *y-ongin*  *y-length*  -4)) 
(setf  (window-stream-x-position  w)  (+  *x-ongin*  -35)) 
(setf  (window-stream-foreground-color  w)  red) 
(format  w  "~s"  *max-revs*) 

(setf  (window-stream-x-position  w)  (+  *x-ongin*  *x-length*  5)) 
(setf  (window -stream-foreground-color  w)  blue) 
(format  w  "+1") 

(setf  (window-stream-v-position  w)  (+  *y-ongin*  (*  *y-length*  0.75)  -3)) 
(setf  (window-stream-x-position  w)  (+  *x-ongin*  -40)) 
(setf  (window-stream-foreground-color  w)  black) 
(format  w  "Revs") 

(setf  (window-stream-x-position  w)  (+  *x-ongin*  *x-length*  5)) 
(format  w  "Revs") 

(setf  (window-stream-y-position  w)  (+  *y-origin*  (*  *y-length*  0.5)  3)) 
(setf  (window-stream-x-position  w)  (+  *x-ongm*  -43)) 
(setf  (window-stream-foreground-color  w)  red) 
(format  w  "~s"  (-  *max-revs*)) 

(setf  (window-stream-x-position  w)  (+  *x-ongin*  *x-length*  5)) 
(setf  (window-stream-foreground-color  w)  blue) 
(format  w  "-1") 
.  omega  labels 

(setf  (window-stream-y-position  w)  (+  *y-origin*  (*  *y-length*  0.5)  -10)) 
(setf  (window-stream-x-position  w)  (+  *x-ongin*  -35)) 
(setf  (window-stream-foreground-color  w)  red) 
(format  w  "~s"  *max-speed*) 

(setf  (window-stream-x-position  w)  (+  *x-origin*  *x-length*  10)) 
(setf  (window-stream-foreground-color  w)  blue) 
(format  w  "~s"  (/  *max-speed*  100)) 

(setf  (window-stream -y-position  w)  (+  *y-origin*  (*  *y-length*  0.25)  -3)) 
(setf  (window-stream-x-position  w)  (+  *x-origin*  -36)) 
(setf  (window -stream-foreground-color  w)  black) 
(format  w  "RPM") 

(setf  (window-stream-x-position  w)  (+  *x-origin*  *x-length*  5)) 
(format  w  "RPM") 

(setf  (window-stream-y-position  w)  (+  *y-origin*  3)) 
(setf  (window-stream-x-position  w)  (+  *x-ongin*  -43)) 
(setf  (window-stream-foreground-color  w)  red) 
(format  w  "~s"  (-  *max-speed*)) 

(setf  ( window-stream-x-position  w)  (+  *x-origin*  *x-length*  5)) 
(setf  (window-stream-foreground-color  w)  blue) 
(format  w  "~s"  (-  (/  *max-speed*  100))) 
;  time  labels 

(setf  (window-stream-foreground-color  w)  black) 
(setf  (window-stream-y-position  w)  (+  *y-origin*  -15)) 
(setf  (window-stream-x-position  w)  (+  *x-origin*  -3)) 
(format  w  "0") 

(setf  (window-stream-x-position  w)  (+  *x-origin*  (/  *x-length*  6)  -7)) 
(format  w"  10") 
(setf  (window-stream-x-position  w)  (+  *x-origin*  (/  *x-length*  3)  -7)) 
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(format  w  "20") 

(setf  (window-stream-x-position  w)  (+  *x-origin*  (/  *x-length*  2)  -7)) 

(format  w  "30") 

(setf  (window -stream-x-position  w)  (+  *x-ongin*  (*  *x-length*  (/  2  3))  -7)) 

(format  w  "40") 

(setf  (window-stream-x-position  w)  (+  *x-ongin*  (*  *x-Iength*  (/  5  6))  -7)) 

(format  w  "50") 

(setf  (window-stream-x-position  w)  (-*-  *x-origin*  *x-length*  -7)) 

(format  w  "60") 

(setf  (window-stream-) -position  w)  (+  *y-ongin*  -30)) 

(setf  (window -stream-x-position  w)  (+  *x-ongin*  -35)) 

(setf  (window -stream-foreground-color  w)  red) 

(format  vv  "motor") 

(setf  (window -stream-x-position  w)  (+  *x-ongin*  *\-length*)) 

(setf  (window-stream-foreground-color  w)  blue) 

(format  w  "joint") 

(setf  (window-stream-foreground-color  w)  black) 

(setf  (window-stream-x-position  w)  (+  *x-ongin*  (*  *x-length*  .5)  -13)) 

(format  w  "time")) 

(dcfmethod  draw-motor-position  ((window  window-stream)  revolutions) 
(draw-point-xy  window 
*x-time-value* 

(+  *y-origin*  (*  0.75  *y-length*) 
(*0.25  *y-length* 
(cond  ((zerop  revolutions)  0) 
((<  revolutions  0) 

(/  (-  (mod  revolutions  *max-revs*) 
*max-revs*) 
*max-revs*)) 
(t  (/  (mod  revolutions  *max-revs*) 
*max-revs*))))) 
color  red)) 

(defmethod  draw -load-position  ((window  window-stream)  revolutions) 
(draw-point-xy  window 
*x-time-value* 

(+  *y-origin*  (*  0.75  *y-length*) 
(*  0.25  *y-length* 
(cond  ((zerop  revolutions)  0) 
((<  revolutions  0) 

(-  (mod  revolutions  1.0)  1.0)) 
(t  (mod  revolutions  1.0))))) 
color  blue)) 
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(defmethod  draw-motor-speed  ((window  window-stream)  speed) 
(draw-point-xy  window 
*x-time-value* 
(+  *y-ongin*  (*  0.25  *y-length*) 

(*  0.25  *y-length*  (/  speed  *max-speed*))) 
color  red)) 

(defmethod  draw-load-speed  ((window  window-stream)  speed) 
(draw-point-xy  window 
*x-time-value* 
(+  *y-origin*  (*  0.25  *y-length*) 

(*  0.25  *y-length*  100  (/  speed  *max-speed*))) 
xolor  blue)) 

(defmethod  update-minutes  ((window  window-stream)  minutes) 
(clear  window) 
(draw-grid  window) 
(self  *  minutes*  minutes)) 

(defmethod  set-x-coord  ((window  window -stream)  seconds) 
(if  (>  (truncate  (/  seconds  60))  *minutes*) 

(update-minutes  window  (truncate  (/  seconds  60)))) 
(self  *x-time-value*  (round  (+  *x-ongin* 

(*  (/ (mod  seconds  60)  60) 
*x-length*))))) 

(defmethod  display-state  ((window  window-stream)  (j  joint)  current-time) 
(set-x-coord  window  current-time) 

(draw-motor-speed  window  (RAD/SECtoRPM  (omega  (prime-mover  j)))) 
(draw-load-speed  window  (RAD/SECtoRPM  (omega  (load-shaft  j)))) 
(draw-motor-position  window  (RADtoREV  (theta  (pnme-mover  j)))) 
(draw-load-position  window  (RADtoREV  (theta  (load-shaft  j))))) 

(setf  *display* 
(make-window-stream 
:left  1 
:  bottom  1 

:width  (+  *x-length*  (*  2  *x-origin*)) 
: height  (+  *y-length*  (*  2  *y-origin*)) 
:backgTound-color  white 

foreground-color  black 
:  inner-region-left 
:  inner-region-bottom 
:  inner-region-width 
:  inner-region-height 

title  "joint  actuator  simulation" 
:activate-p  t)) 

(setf  *minutes*  0) 
(draw-grid  *display*) 
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;  "time.routines.cl" 


(dcfun  reset-system-time  () 
(setf  *RefTime*  (get-internal-real-timoi) 

(defun  system-time  0 
(/ (-(get-internal-real-time)  *RefTime*)  1000.0)) 

(defun  delta-time  (time) 
(-  (system-time)  time)) 

(reset-svstem-time) 


;  "math. routines. cl" 


(defun  sqr  (x)  (*  x  x)) 

(defun  sgn  (x) 
(if(<xO)-l  1)) 

(defun  RPMtoRAD/SEC  (rpm) 
(*  rpm  (/pi  30)))  ;  *  2pi/6() 

(defun  RAD/SECtoRPM  (rad/sec) 
(*  rad/sec  (/ 30  pi))) 

(defun  REVtoRAD  (rev) 
(*  2  pi  rev)) 

(defun  RADtoREV  (rad) 
(/  rad  (*  2  pi))) 
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APPENDIX    B 


SOURCE  CODE  FILES  (untested) 
//fileMmotor.h" 


#ifndef_MOTOR_H 
#define  _MOTOR_H 

#include  <math  h> 
#include  <stdio.h> 

class  motor  {    //  inertia-less  motor  class 
private: 

float  Fc;      //  Coulomb  friction  constant 

float  Fv;      //  Viscous  friction  constant 

float  Kt;      //  Torque  constant 

float  Kb;      //  Back  EMF  constant 

float  Ra;      //  Armature  resistance 

float  BDm;     //  Rated  brush  brop  value 

float  BDc;     // Ln(l/2)/BDh 

//  BDh  =  Voltage  applied  such  that  brush  drop  =  1/2  BDm 

//  subtracts  brush  drop  from  source  voltage 

float  AppliedVoltage(float); 

public: 

float  la;      //  Armature  current  (available  for  current  feedback) 

motor()  {} 

//  called  after  constructor  for  initialization 

void  init_motor(float  TorqueConstant,  //  N*m/ Ampere 

float  BackEMFConstant,  //  Volts/RPM 

float  NoLoadCurrent,  //  Amperes 

float  NoLoadSpeed        =  1000.0,  //  RPM 
float  StartingCurrent    =    0.0,  //  Amperes 
float  ArmatureResistance  =     1.0, //Ohms 
float  RatedBrushDrop      =     2.0,  //  Volts 
float  HalfBrushDrop       =     3.0);// Volts 

float  DevelopedTorque(float,  float); 

}; 
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//  provide  initialization  for  specific  motor  t\pes  in  Aquarobot 
void  makeRA20(motor&  m); 
void  makeRH25(motor&  m), 

#endif 
/*  EOF  */ 


//file  "motor.c" 


#include  "motor. h" 

void  motor  imt_motor(float  TorqueConstant. 

float  BackEMFConstant. 

float  NoLoadCurrent, 

float  NoLoadSpeed, 

float  StartingCurrent. 

float  ArmatureResistance, 

float  RatedBrushDrop. 

float  HalfBrushDrop) 
{ 
la  =  0.0; 

Fc  =  TorqueConstant  *  StartingCurrent; 
Fv  =  (TorqueConstant  *  NoLoadCurrent  -  Fc)  /  NoLoadSpeed; 

Kt  =  TorqueConstant; 
Kb  =  BackEMFConstant; 
Ra  =  ArmatureResistance; 

BDm  =  RatedBrushDrop; 
if(BDm<0.0)  { 

printf("error:  rated  brush  drop  must  be  >=  0  Volts... \n"); 

printf("default  brush  drop  value  (0  Volts)  used.An"); 

BDm  =  0.0; 

BDc=  1.0; 

} 

else  if  (BDm  =  0.0)  { 
BDc=  1.0; 

} 

else  if  (HalfBrushDrop  <  BDm/2)  { 

printf("error:  half  brush  drop  must  be  >=  1/2  rated  brush  drop.  An"); 

printfC'defaulted  to  1/2  RatedBrushDrop... \n"); 

BDc  =  log(0.5)*2.0/BDm; 
} 
else  { 

BDc  =  log(0  5)  /  HalfflrushDrop; 
} 
} 
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float  motor:  :DevelopedTorque(float  Source  Voltage,  float  omega) 

{ 

float  Torque; 
float  FnctionLoss; 

//  determine  armature  current  and  save 

la  =  ( Applied  Voltage(  Source  Voltage)  -  (Kb  *  omega))  /  Ra; 

//  determine  motor  torque 
Torque  =  Kt  *  la; 

//  calculate  loss 

//  loss  opposes  omega  (viscous  and  coulomb  components) 

if      (omega  >  0.0)  FnctionLoss  =  omega  *  Fv  +  Fc; 

else  if  (omega  <  0.0)  FnctionLoss  =  omega  *  Fv  -  Fc; 

//  if  (omega  ==  0)  :  loss  opposes  Torque  (no  viscous  component) 

else  if  (Torque  >  Fc)  FnctionLoss  =    Fc; 

else  if  (Torque  <  -Fc)  FnctionLoss  =  -  Fc; 

//  if  ((omega  ==  0)  &&  (|Torque|  <  Fc))  :  Torque  insufficient  to  overcome  Fc 

else  FnctionLoss  =  MotorTorque; 

return  (Torque  -  FnctionLoss); 
} 
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/*  Private  Function  */ 

float  motor:  Applied  Voltage!  float  Vs) 

» 

// return  Vs(l  -  (BDm/|Vs|)*(l  -exp(ln(l/2)*|Vs|/BDh))) 
if(Vs  =  0.0){ 

return  0.0; 

i 
/ 

else  if  (Vs  <  0.0)  {  //  negative  Vs 
return  (Vs  +  (BDm  *  ( 1  -  exp(-BDc*Vs)))); 

else  {  //  positive  Vs 

return  ( Vs  -  (BDm  *  (1  -  e\p(  BDc*Vs)))); 

} 

/*  specific  Aquarobot  motor  type  initializations  */ 

//  Aquarobot  motor  parameters  for  mitmotor 

//  read  from  spec  sheet  provided;  includes  harmonic  gear 

#define  RA20_PARAMETERS  32.0.  3.4,  0  78.  25.0.  0.32.  3.2,  2.0.  3.0 

#define  RH25_PARAMETERS  33.0,  3.5,0.89,  25.0.  0  48.  11.  2.0.  3.0 

void  makeRA20(motor&  m) 

{ 
m.init_motor(RA20_PARAMETERS); 


void  makeRH25(motor&  m) 

{ 
m.init_motor(RH25_PARAMETERS); 

} 

I*  EOF  */ 
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//file  "amplifierclipper.h" 


#ifndef  _AMP_CLIP_H_ 
#define  _AMP_CLIP_H_ 

class  amplifier_clipper  { 

private: 
float  gain; 
float  maxvalue. 
float  minvalue, 

public: 
amplifier_clipper()  { } 
//  call  after  constructor  for  initialization 
void  init_amplifier_clipper( float  g,  float  max_v,  float  min_v); 

float  amplifv(float  inputvalue); 

}; 

class  aquadnver :  private  amplifierclipper  { 
private: 

float  theta_gain; 

float  omegagain; 

float  currentgain; 

public: 
aqua_dnver()  {} 

//  call  after  constructor  for  initialization 
init_aqua_dnver(float  displacement_gain, 

float  velocity_gain, 

float  current_feedback_gain); 
float  drive(  float  thetaerror,  float  omega,  float  current); 

}; 

//  provide  initialization  for  specific  joint  on  aqualeg 
void  makeJldnver(aqua_driver&  d); 
void  makeJ2driver(aqua_driver&  d); 
void  makeJ3driver(aqua_driver&  d); 

#endif 
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//  file  "amplifierclipper.c" 


#include  "amplifierclipper.h" 

void 

init_amplifier_clipper( float  g,  float  max_v.  float  min_v) 

{ 

gain       =  g; 

max_value  =  max_v; 

min_value  =  minv, 
} 

float 

amplifierclippcr:  :amplify(  float  inputvalue) 

float  outputvalue  =  gain  *  inputvalue; 

if     (nunvalue  >  outputvalue) 

return  min_value; 
elsif  (maxvalue  <  outputvalue) 

return  maxvalue; 
else 

return  outputvalue; 
} 

aquadnver:  :imt_aqua_dnver(float  displacementgain, 
float  velocity_gain, 
float  currentfeedbackgain) 

{ 

//  set  final  gain  to  1  and  clip  at  +/-  75  VDC 
init_amplifier_clipper(1.0,  75.0,  -75.0); 

thetagain    =  displacementgain; 
omegagain    =  velocity  gain; 
currentgain  =  currentfeedbackgain; 
} 

float 

aqua_driver::drive(float  thetaerror,  float  omega,  float  current) 

{ 
return  (amplify(  thetagain    *  thetaerror 

-  omegagain    *  omega 

+  currentgain  *  current)); 
} 
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//  driver  specs  (theta,  omega,  current) 

#define  SHOULDER_DRIVER_GAINS  150.0,  5  0.  0.5 

#define  KNEE1_DRIVER_GAINS  150.0,  5.0,  0.5 

#defme  KNEE2_DRIVER_GAINS  150.0,  5.0,  0.5 

void  makeJldnver(aqua_dnver&  d) 

X 

d.init_aqua_driver(SHOULDER_DRIVER_GAINS); 

) 

void  m;ikeJ2dnver(aqua_dnver&  d); 
d.mit_aqua_driver(KNEEl_DRIVER_GAINS): 


void  makeJ3dnver(aqua_dnver&  d); 

{ 
d.init_aqua_dnver(KNEE2_DRlVER_GAINS); 

} 


//  file  "jointactuator.h" 


#ifndef_JA_H_ 
#define  _JA_H_ 

#include  <> 
#include  "" 

class  aquajointactuator  { 
protected: 

float  orderedtheta; 

ampliflerclipper  daconverter; 

amplifierclipper  fvconverter; 

aqua_dnver        d; 

motor  m; 

public: 
aquajoint_actuator()  {} 
void  reset(float  theta); 
void  input_order(float  deltatheta); 
float  torque(float  currenttheta,  float  currentomega); 

}; 

class  shoulderacruator  :  public  aquajointactuator  { 

public: 
shoulder_actuator(float  theta); 

}; 
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class  kneel  actuator    public  aquajointactuator  { 

public: 

knee l_actuator( float  theta); 

float  torque!  float  current  theta.  float  currentomega); 
V. 

class  knee2  actuator    public  aquajointactuator  { 

public: 
knee2_actuator( float  theta). 
float  torque(  float  current  Jheta,  float  currcnt_omega); 


#endif 


//  file  '  jointactuator.c" 

#include  "joint  jictuator.h" 

//  aquajoint_actuator  (parent  class)  functions 

void 

aquajoint_actuator::reset( float  theta), 

{ 

ordered  theta  =  theta; 
m.la  =  0.0; 

} 

void 

aqua  jointactuator:  :input_order(  float  deltatheta); 

{ 
orderedtheta  +=  deltatheta; 

} 

float 

aquajoint_actuator::torque(float  currenttheta,  float  currentomega); 
{ 

float  souicevoltage; 
sourcevoltage  =  d->drive 

(da_converter->amplify(current_theta  -  orderedtheta), 

fv_converter->amplify(current_omega), 

m.Ia); 

return  (m->de\eloped_torque(source_voltage)), 
} 
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//  demved  class  specs 

//  output  10  volts  for  6144  pulse  count 

#define  DA_CONVERTER_RATIO  10.0/6144.0 

//  100  pulses  drive  the  motor  1  revolution 
#define  PULSES_PER_REV  100.0 

//  output  3  volts  per  1000  RPMs 

#define  FV_CONVERTER_RATIO  3.0/1000.0 

//  harmonic  gear  only  for  shoulder 
#define  SHOULDER_REDUCTION  161.0 

//  harmonic  and  bevel  gears  for  knees 
#defineKNEEl_REDUCTION  160.0*3.0 
#define  KNEE2REDUCTION  160.0*2.0 

//  derrived  class  functions 

shoulderactuator:  :shoulder_actuator(  float  theta) 

{ 

ordered_theta  =  theta; 
da_converter->init_amplifier_clipper 

(SHOULDER_REDUCTION  *  PULSES_PER_REV  *  DA_CONVERTER_RATIO,  10.0.  -10.0); 
fv_converter->init_amplifier_clipper 

(SHOULDER_REDUCTION  *  FV_CONVERTER_RATIO.  10.0,  -10.0); 
makeJ  1  dnver(&d); 
makeRA20(&m); 
} 

knee  1  actuator: : knee  1  _actuator(  float  theta) 

{ 

orderedtheta  =  theta; 
da_converter->init_amplifier_clipper 

(KNEE1_REDUCTI0N  *  PULSES_PER_REV  *  DA_CONVERTER_RATIO,  10  0.  -10.0); 
fv_converter->irut_amplifier_clipper 

(KNEE1_REDUCTI0N  *  FV_CONVERTER_RATIO,  10.0.  -10.0); 
makeJ2driver(&d); 
makeRH25(&m); 
} 

float 

kneel_actuator::torque(float  currenttheta,  float  current_omega) 

{ 
return  (3.0  *  aquaJoint_actuator::torque(current_theta,  current_omega/3.0)); 

} 
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knee2_actuator:  knee2_actuator(  float  theta) 

{ 

orderedtheta  =  theta; 
da_converter->init_amplifier_clipper 

(KNEE2_REDUCTION  *  PULSES_PER_REV  *  DA  CONVERTER_RATIO.  10.0,  -10  0); 
fv_converter->init_amplifier_clipper 

(KNEE2_REDUCTION  *  FVCONVERTERRATIO.  10  0.  -10  0); 
make  J3  driver  (&d); 
makeRH25(&m); 


float 

knee2_actuator::torque(  float  currenttheta.  float  cuirentomega) 

; 
\ 

return  (2.0  *  aquajointactuator  torque(current_theta.  current_omega/2.0)); 
} 


85 


APPENDIX    C 


LOADING  AND  OPERATING  INSTRUCTIONS 


To  run  demo,  start  LISP  Interpreter  and  call: 

(load  "aqua-loader") 
This  file  loads  the  source  code  in  the  correct  sequence  and  makes  calls  to  run  the  demo. 
Additional  runs  may  be  observed  by  calling: 

(drop-aqua) 


SOURCE  CODE  FILES 

;  "aqua-loader" 

;  LOADER  FOR  AQUA-ROBOT 

;  define  loader/compiler  functions 

(load  "load-files.cl") 

aqua-robot  loader/compiler  functions 

(load-aqua) 
( load-compiled-aqua) 
;  (compile-and-load-aqua) 

(defun  drop-aqua  () 
(restart-aqua) 
(do  0  (nil) 

(dotimes  (i  *loops*)  (update-aquarobot  aqua-1)) 

(new-picture))) 

(aqua-picture) 
(drop-aqua) 
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;  "load-files.cl" 


(dcfun  load-aqua  () 
;  general  purpose  files 
(load  "misc.cl") 
(load  "vector  cl") 
(load  "matrix. cl") 
(load  "kinematics. cl") 
(load  "rigid-body  cl") 
(load  "strobe-camera. cl") 
(load  "link.cl") 
.  aqua-robot  specific  files 
(load  "aqua-data. cl") 
(load  "aqua-link. cl") 
(load  "aqua.cl") 
(load  "aqua-leg. cl") 
(load  "aqua-in\ -kinematics. cl") 
(load  "aqua-jacobian.cl") 
( load  "aqua-update-forces-and-torques.cl")) 

(defun  load-compilcd-aqua  () 
(load  "misc.  fasl") 
(load  "vector. fasl") 
(load  "matrix. fasl") 
(load  "kinematics. fasl") 
(load  "rigid-body  fasl") 
(load  "strobe-camera. fasl") 
(load  "link. fasl") 
(load  "aqua-data. fasl") 
(load  "aqua-link. fasl") 
(load  "aqua. fasl") 
(load  "aqua-leg. fasl") 
(load  "aqua-inv-kinematics.fasl") 
(load  "aqua -jacobian  fasl") 
(load  "aqua-update-forces-and-torques.fasl")) 
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(defun  compile-and-load-aqua  () 
,  (compile-file  "misc.cl") 

(load  "misc. fasl") 
.  (compile-file  "vector. cl") 

(load  "vector. fasl") 
;  (compile-file  "matrix. cl") 

(load  "matrix. fasl") 

(compile-file  "kinematics  cl") 

(load  "kinematics  fasl") 

(compile-file  "ngid-body  cl") 

(load  "rigid-body  fasl") 

(compile-file  "strobe-camera. cl") 

(load  "strobe-camera  fasl") 

(compile-file  "link  cl") 

(load  "link. fasl") 

(compile-file  "aqua-data. cl") 

(load  "aqua-data. fasl") 

(compile-file  "aqua-link. cl") 

(load  "aqua-link. fasl") 

(compile-file  "aqua.cl") 

(load  "aqua. fasl") 

(compile-file  "aqua-leg. cl") 

(load  "aqua-leg. fasl") 

(compile-file  "aqua-inv-kinematics.cl") 

(load  "aqua-inv-kinematics.fasl") 

(compile-file  "aqua-jacobian.cl") 

(load  "aqua-jacobian.fasl") 

(compile-file  "aqua-update-forces-and-torques.cl") 

(load  "aqua-update-forces-and-torques.fasl")) 
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;  "misc.cl' 


(dcfun  atan2  (d.\  dy) 
(cond  ((zerop  dx)  (cond  ((/crop  dy)  0.0) 
((<dy0)    (-  (*  0.5  pi))) 
(t  (*  0.5  pi)))) 

((<d.\0)    (cond  ((<  d\  ())    (- (atan  (/dy  dx))  pi)) 

(t  (+ (atan  (/ dy  dx))  pi)))) 

(t  (atan(/d>  dx))))) 

.  returns  angle  in  degrees. 

(dcfun  atan2d  (dx  dy)  (rad-to-deg  (atan2  dx  dy))) 

(dcfun  sqr(x)  (*  x  x)) 

(defconstant  rad-to-deg-multiplicr  (/  180  pi)) 
(defun  rad-to-deg  (rad)  (*  rad  rad-to-deg-multiplien) 

(defconstant  deg-to-rad-multiplier  (/pi  180)) 

(defun  deg-to-rad  (deg)  (*  deg  deg-to-rad-niultiplier)) 

;Retuins  first  n  elements  of  list 
(defun  near  (n  list) 
(cond  ((zerop  n)  nil) 

(t  (cons  (car  list)  (near  (1-  n)  (cdr  list)))))) 


89 


;  "vector.cl" 

,  A  vector  is  a  list  of  numerical  atoms. 

(defun  vector-add  (vector-1  vector-2)  (mapcar '+  vector-1  vector-2)) 

(defun  vector-subtract  (vector-1  vector-2)  (mapcar '-  vector-1  vector-2)) 

(defun  scalar-multiply  (scalar  vector) 
(cond  ((null  vector)  nil) 

(t  (cons  (*  scalar  (car  vector)) 

(scalar-multiply  scalar  (cdr  vector)))))) 

(defun  dot-product  (x  y) 
(apply  '+  (mapcar  '*  x  y)))      ;A  matrix  is  a  list  of  row  vectors. 

(defun  cross-product  (x  y)         ,x  and  y  are  3D  vectors, 
(list  (-  (*  (cadr  x)  (caddr  y))  (*  (caddr  x)  (cadr  y))) 
(-  (*  (caddr  x)  (car  y))    (*(carx)    (caddr  y))) 
(-  (*  (car  x)    (cadr  y))  (*  (cadr  x)  (car  y))))) 

(defun  vector-length  (vector)  (sqrt  (dot-product  vector  vector))) 

(defun  distance-between  (x  y)  ;points  x  and  y  represented  by  vectors, 
(vector-length  (vector-subtract  x  y))) 

;  returns  a  vector  (O*(one-position  -1)1  O*(length-one-position)) 
(defun  umt-vector  (one-position  length) 
(do  ((n  length  (1-n)) 

(vector  ml  (cons  (cond  ((=  one-position  n)  1)  (t  0))  vector))) 
((zerop  n)  vector))) 

(defun  append  1  (L)  (append  L  '(1))) 
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;  "matrix.cl' 


;  requires  VICTOR.  CL 
.  requires  MISC.CL  "near" 

;  A  matrix  is  a  list  of  row  vectors. 

(defun  transpose  (A) 
(cond  ((null  (cdr  A))  (mapcar  'list  (car  A))) 

(t  (mapcar  'cons  (car  A)  (transpose  (cdr  A)))))) 

(defun  post-multiply  (M  x)     ;M  is  a  square  matrix,  x  is  a  conformable  vector 
(cond  ((null  (cdr  M))  (list  (dot-product  (car  M)  x))) 

(t  (cons  (dot-product  (car  M)  x)  (post-multiply  (cdr  M)  x))))) 

(defun  pre-multiply  (vector  matrix) 
(post-multiply  (transpose  matrix)  vector)) 

(defun  matrix-multiply  (A  B)       ,A  and  B  are  conformable  matrices 
(cond  ((null  (cdr  A))  (list  (pre-multiply  (car  A)  B))) 

(t  (cons  (pre-multiply  (car  A)  B)  (matrix-multiply  (cdr  A)  B))))) 

(defun  chain-multiply  (L)         ,L  is  a  list  of  names  of  conformable  matrices 
(cond  ((null  (eddr  L))  (matrix-multiply  (eval  (car  L))  (eval  (cadr  L)))) 
(t  (matrix-multiply  (eval  (car  L))  (chain-multiply  (cdr  L)))))) 

(defun  cycle-left  (matrix)  (mapcar  'row-cycle-left  matrix)) 

(defun  row-cycle-left  (row)  (append  (cdr  row)  (list  (car  row)))) 

(defun  cycle-up  (matrix)  (append  (cdr  matrix)  (list  (car  matrix)))) 

(defun  unit-matnx  (size) 
(do  ((row-number  size  (1-  row-number)) 

(I  nil  (cons  (unit-vector  row-number  size)  I))) 
((zerop  row-number)  I))) 

(defun  concat-matnx  (A  B)    ,A  and  B  are  matrices  with  equal  number  of  rows 
(cond  ((null  A)  B) 

(t  (cons  (append  (car  A)  (car  B))  (concat-matnx  (cdr  A)  (cdr  B)))))) 

(defun  augment  (matrix) 
(concat-matnx  matnx  (unit-matnx  (length  matrix)))) 

(defun  normalize-row  (row)  (scaJar-multiply  (/  1.0  (car  row))  row)) 
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(defun  solve-first-column  (matnx)         .Reduces  first  column  to  ( 1  0      0). 
(do*  ((LI  matrix  (cdr  LI)) 

(L2  (normalize-row  (car  matnx))) 
(L3  (list  L2)  (cons  (vector-add  (car  LI) 

(scalar-multiply  (-  (caar  LI))  L2))  L3))) 
((null  (cdr  LI))  (reverse  L3)))) 

(defun  square-car  (M)  ;Returns  square  matnx  extracted  from  front  of  matrix  M. 
(do  ((m  (length M)) 
(LI  M  (cdr  LI)) 

(L2  nil  (cons  (near  m  (car  LI))  L2))) 
((null  LI)  (reverse L2)))) 

;;L  is  a  list  of  lists.  This  function  finds  list  with 
; largest  car  and  moves  it  to  head  of  list  of  lists, 
(defun  max-car-first  (L) 
(cond  ((null  (cdr  L))  L) 

(t  (if  (>  (abs  (caar  L))  (abs  (caar  (max-car-first  (cdr  L)))))  L 
(append  (max-car-first  (cdr  L))  (list  (car  L))))))) 

;  Applies  max-car-first  to  first  n  elements  of  list, 
(defun  nmax -car-first  (n  list) 
(append  (max -car-first  (near  n  list))  (nthedr  n  list))) 

(defun  matnx-inverse  (M) 
(do  ((Ml  (max-car-first  (augment  M)) 
(cond  ((null  Ml)  nil) 

(t  (nmax -car-first  n  (cycle-left  (cycle-up  Ml)))))) 
(n  (1-  (length  M))(l-n))) 

((or  (rrunusp  n)  (null  Ml))  (cond  ((null  Ml)  nil)  (t  (square-car  Ml)))) 
(setq  Ml  (cond  ((zerop  (caar  Ml))  nil)  (t  (solve-first-column  Ml)))))) 
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'  kinematics. cl" 


;  requires  MATRIX. CL 

(defun  dh-matrix  (cosrotate  sinrotate  costwist  sintwist  length  translate) 
(list  (list  cosrotate  (-  (*  costwist  sinrotate)) 

(*  sintwist  sinrotate)  (*  length  cosrotate)) 
(list  sinrotate  (*  costwist  cosrotate) 

(-  (*  sintwist  cosrotate))  (*  length  sinrotate)) 
(list  0.  sintwist  costwist  translate)  (list  0.  0  0    1  ))) 

(defun  mdh-matrix  (cosrotate  sinrotate 
costwist-i-1  sintwist-i-1 
length-i-1  translate) 
(list  (list  cosrotate  (-  sinrotate)  0.  length-i-1) 

(list  (*  sinrotate  costwist-i-1)  (*  cosrotate  costwist-i-1) 

(-  sintwist-i-1)  (-  (*  sintwist-i-1  translate))) 
(list  (*  sinrotate  sintwist-i-1)  (*  cosrotate  sintwist-i-1) 

costwist-i-1  (*  costwist-i-I  translate)) 
(list  0.0.  0.  1.))) 

(defun  homogeneous-transform  (azimuth  elevation  roll  x  y  z) 
(rotation-and-translation  (sin  azimuth)  (cos  azimuth)  (sin  elevation) 
(cos  elevation)  (sin  roll)  (cos  roll)  x  y  z)) 

(defun  rotation-and-translation  (spsi  cpsi  sth  cth  sphi  cphi  x  y  z) 
(list  (list  (*  cpsi  cth)  (-  (*  cpsi  sth  sphi)  (*  spsi  cphi)) 
(+  (*  cpsi  sth  cphi)  (*  spsi  sphi))  x) 
(list  (*  spsi  cth)  (  +  (*  cpsi  cphi)  (*  spsi  sth  sphi)) 

(-  (*  spsi  sth  cphi)  (*  cpsi  sphi))  y) 
(list  (-  sth)  (*  cth  sphi)  (*  cth  cphi)  z) 
(list  0.0.0.  1.))) 

(defun  inverse-H  (H) 
(let*  ((minus-P  (list  (-  (fourth  (first  H))) 
(-  (fourth  (second  H))) 
(-  (fourth  (third  H))))) 
(inverse-R  (transpose  (square-car  (reverse  (rest  (reverse  H)))))) 
(inverse-P  (post-multiply  inverse-R  minus-P))) 
(append  (concat-matnx  inverse-R  (transpose  (list  inverse-P))) 
(list  (list  0  0  0  1))))) 
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;  "rigid-body.cl" 


;  requires  KINEMATICS. CL 

(defclass  rigid-body     () 
((location  .The  three-vector  (x  y  z)  in  world  coordinates. 

:imtarg  :  location 

accessor  location) 
(velocity  ;The  six-vector  (uvwpq  r)  in  body  coordinates. 

mitarg  : velocity 

accessor  velocity) 
(acceleration  The  vector  (u-dot  v-dot  w-dot  p-dot  q-dot  r-dot). 

accessor  acceleration) 
(forces-and-torques    ;The  vector  (Fx  Fy  Fz  L  M  N)  in  body  coordinates. 

accessor  forces-and-torques) 
(moments-of-inertia    ;The  vector  (Ix  Iy  Iz)  in  principal  axis  coordinates. 

mitarg  :moments-of-inertia 

:accessor  moments-of-inertia) 
(mass 

:initarg  :mass 

accessor  mass) 
(node-list  ;List  of  vertices  for  wire  frame  model 

initarg  node-list 
: accessor  node-list) 
(polygon-list  ;Sets  of  above  vertices  defining  polygons 

mitarg  :polygon-list  ;Ex:'((l  2  3)(2  3  4  5)(4  5  6)) 

accessor  polygon-list) 
(transformed-node-list 
: accessor  transformed-node-list) 
(H-matrix 
:accessor  H-matrix) 
(current-time 

accessor  current-time))) 

(defmethod  move  ((body  rigid-body)  azimuth  elevation  roll  x  y  z) 
(serf  (H-matnx  body) 

(homogeneous-transform  azimuth  elevation  roll  x  y  z)) 
(transform-node-list  body) 
(update-position  body)) 
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(dcfmethod  move-incremental  ((tody  rigid-body)  increment-list) 
(setf  (H-matrix  body) 

(matnx-multiply  (H-matrix  body)  (homogeneous-transform 

(first  increment-list) 

(second  increment-list) 

(third  increment-list) 

(fourth  increment-list) 

(fifth  increment-list) 

(sixth  increment-list)))) 
(transform-node-list  bod>) 
(update-position  body)) 

(defmethod  get-delta-t  ((body  rigid-body)) 
(let*  ((new-time  (get-internal-real-time)) 

(delta-t  (/  (-  new-time  (current-time  body))  1000))) 
(setf  (current-time  body)  new-time) 
delta-t)) 

(defmethod  start-timer  ((body  rigid-body)) 
(setf  (current-time  body)  (get-internal-real-time))) 

(defmethod  update-ngid-body  ((body  ngid-body))      ;Euler  integration 
(let*  ((delta-t  (get-delta-t  body))) 
(update-H-matnx  body  delta-t) 
(transform-node-list  body) 
(update-position  body) 
(update-velocity  body  delta-t) 
(update-acceleration  body))) 

(defmethod  update-acceleration  ((tody  rigid-body)) 
(setf  (acceleration  body)  .(list  u-dot  v-dot  w-dot  p-dot  q-dot  r-dot) 
(multiple-value-bind  ;Assumes  principal  axis 

(Fx  Fy  Fz  L  M  N    u  v  w  p  q  r    Ix  Iy  Iz)  .coordinates  with  origin  at 
(values-list  xenter  of  gravity  of  body 

(append 

(forces-and-torques  body)  (velocity  body)  (moments-of-inertia  body))) 
(list  (+  (*  v  r)  (*  -1  w  q)  (/  Fx  (mass  body)) 

(*  *gravity*  (first  (third  (H-matrix  body))))) 
(+  (*  w  p)  (*  -1  u  r)  (/  Fy  (mass  body)) 

(*  *gravity*  (second  (third  (H-matnx  body))))) 
(+  (*  u  q)  (*  -1  v  p)  (/  Fz  (mass  body)) 
(*  'gravity*  (third  (third  (H-matnx  body))))) 
(/  (+  (*  (-  Iy  Iz)  q  r)  L)  Ix) 
(/  (+  (•  (-  Iz  Ix)  r  p)  M)  Iy) 
(/  (+  (*  (-  Ix  Iy)  p  q)  N)  Iz))))) 
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(defmethod  update-velocity  ((body  ngid-body)  delta-t) 
(serf  (velocity  body) 
(vector-add  (velocity  body) 

(scalar-multiply  delta-t  (acceleration  body))))) 

(defmethod  update-H-matnx  ((body  rigid-body)  delta-t) 
(serf  (H-matrix  body) 
(matrix-multiply 

(H-matnx  body)  (homogeneous-transform 
(*  delta-t  (sixth  (velocity  body))) 
(*  delta-t  (fifth  (velocity  body))) 
(*  delta-t  (fourth  (velocity  body))) 
(*  delta-t  (first  (velocity  body))) 
(*  delta-t  (second  (velocity  body))) 
(*  delta-t  (third  (velocity  body))))))) 

(defmethod  transform-node-list  ((body  rigid-body)) 
(setf  (transformed-node-list  body) 
(mapcar  #'( lambda  (node-location) 

(post-multiply  (H-matnx  body)  node-location)) 
(node-list  body)))) 

(defmethod  update-position  ((body  rigid-body)) 
(setf  (location  body)  (near  3  (first  (transformed-node-list  body))))) 

;  (defconstant  ^gravity*  32.2185) 

(defmethod  world-to-body  ((body  ngid-body)  xyz-pos) 
(near  3  (post-multiply  (inverse-H  (H-matrix  body)) 
(append  xyz-pos  '(!))))) 

(defmethod  body-to-world  ((body  rigid-body)  xyz-pos) 
(near  3  (post-multiply  (H-matnx  body)  (append  xyz-pos  '(1))))) 
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"strobe-camera.cl" 


;  requires  RIGID-BODY.CL 

(require  :xcw) 
(cwinitialize-common-windows) 

(dcfclass  strobe-camera  (ngid-body) 
((focal -length 
accessor  focal-length 
lnitform  6) 
(camera-window 
accessor  camera-window 
lnitform  (cw:  make-window -stream  borders  5 
left  500 
bottom  500 
:width  300 
height  300 

title  "strobe-camera-image" 
:activate-p  t)) 
(H-matn\ 

lnitform  (homogeneous-transform  .3-30  -300  -90  -90)) 
(inverse-H-matnx 
accessor  inverse-H-matnx 

:initform  (inverse-H  (homogeneous-transform  .3  -.3  0  -300  -90  -90))) 
(enlargement-factor 
accessor  enlargement-factor 
:initform  30))) 

(defmethod  erase-camera-window  ((camera  strobe-camera)) 
(cwclear  (camera-window  camera))) 

(defmethod  move  ((camera  strobe-camera)  azimuth  elevation  roll  xyz) 
(setf  (H-matrix  camera)  (homogeneous-transform  azimuth  elevation  roll  x  y  z)) 
(setf  (inverse-H-matnx  camera)  (inverse-H  (H-matnx  camera)))) 

(defmethod  take-picture  ((camera  strobe-camera)  (body  rigid-body)) 
(let  ((camera-space-node-list  (mapcar  #'(lambda  (node-location) 
(post-multiply  (inverse-H-matnx  camera)  node-location)) 
(transformed-node-list  body)))) 
(dolist  (polygon  (polygon-list  body)) 
(clip-and-draw -polygon  camera  polygon  camera-space-node-list)))) 
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(defmethod  clip-and-draw-polygon 
((camera  strobe-camera)  polygon  node-coord-list) 
(do*  ((initial-point  (nth  (first  polygon)  node-coord-list)) 
(from-point  initial-point  to-point) 
(remaimng-nodes  (rest  polygon)  (rest  remaining-nodes)) 
(to-point  (nth  (first  remaimng-nodes)  node-coord-list) 
(if  (not  (null  (first  remaimng-nodes))) 

(nth  (first  remaining-nodes)  node-coord-list)))) 
((null  to-point) 

(draw -clipped-projection  camera  from-point  initial-point)) 
(draw-clipped-projection  camera  from-point  to-point))) 

(defmethod  draw -clipped-projection  ((camera  strobe-camera)  from-point  to-point) 
(cond  ((and  (<=  (first  from-point)  (focal-length  camera)) 
(<=  (first  to-point)  (focal-length  camera)))  nil) 
((<=  (first  from-point)  (focal-length  camera)) 
(draw-line-in-camera-window  camera 
(perspective-transform  camera  (from-clip  camera  from-point  to-point)) 
(perspective-transform  camera  to-point))) 
((<=  (first  to-point)  (focal-length  camera)) 
(draw-line-in-camera-window  camera 
(perspective-transform  camera  from-point) 

(perspective-transform  camera  (to-clip  camera  from-point  to-point)))) 
(t  (draw-line-in-camera-window  camera 
(perspective-transform  camera  from-point) 
(perspective-transform  camera  to-point))))) 

(defmethod  from-clip  ((camera  strobe-camera)  from-point  to-point) 
(let  ((scale-factor  (/  (-  (focal-length  camera)  (first  from-point)) 
(-  (first  to-point)  (first  from-point))))) 
(list  (+  (first  from-point) 

(*  scale-factor  (-  (first  to-point)  (first  from-point)))) 
(+  (second  from-point) 

(*  scale-factor  (-  (second  to-point)  (second  from-point)))) 
(+  (third  from-point) 
(*  scale-factor  (-  (third  to-point)  (third  from-point))))  1))) 

(defmethod  to-clip  ((camera  strobe-camera)  from-point  to-point) 
(from-clip  camera  to-point  from-point)) 

(defmethod  draw-line-in-camera-window  ((camera  strobe-camera)  start  end) 
(cw:draw-line  (camera-window  camera) 

(cw: make-position  :x  (first  start)  :y  (second  start)) 
(cw: make-position  :x  (first  end)  :y  (second  end)) 
:brush-width  0)) 
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(defmethod  perspective-transform  ((camera  strobe-camera)  point-m-camera-space) 
(let*  ((enlargement-factor  (enlargement-factor  camera)) 
(focal-length  (focal-length  camera)) 

(x  (first  point-in-camera-space))  ;x  axis  is  along  optical  axis 
(y  (second  point-in-camera-space))  ;y  is  out  right  side  of  camera 
(z  (third  point-in-camera-space)))  ;z  is  out  boitom  of  camera 
(list  (+  (round  (*  enlargement-factor  (/  (*  focal-length  y)  x))) 
150)  .to  right  in  camera  window 

(+  150  (round  (*  enlargement-factor  (/  (*  focal-length  (-  z))  \n 
)))))  ;up  in  camera  window 
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;  "link.cl" 


;  requires  RIGID-BODY.CL 

(defclass  link  (rigid-body) 
((motion-limit-flag 

initform  nil 

accessor  motion-limit-flag) 
(twist-angle 

initarg  :twist -angle 

accessor  twist-angle) 
(link-length 

initarg  :link-length 

accessor  link-length) 
(inboard-joint-angle 

initarg  inboard-joint-angle 

:accessor  inboard-joint-angle) 
(inboard-joint-displacement 

initarg  :  inboard-joint-displacement 
:accessor  inboard-joint-displacement) 
(inboard-link 
: initarg  inboard-link 

accessor  inboard-link) 
(A-matrix 

accessor  A-matrix) 
;  added  for  mdh 
(twist-angle-i-1 

initarg  : twist-angle-i-1 

: accessor  twist-angle-i-1) 
(link-length-i-1 

initarg  : link-length-i-1 
:accessor  link-length-i-1) 
(T-matrix 
:accessor  T-matrix))) 

(defclass  rotary-link  (link) 
((min-joint-angle 
:  initarg  :min-joint-angle 
:accessor  min-joint-angle) 
(max-joint-angle 
: initarg  max-joint-angle 
accessor  max-joint-angle))) 
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(defclass  sliding-link  (link) 
((min-joint-displaccment 
imtarg  min-joint-displacement 
accessor  min-joint-displacemcnt) 
(max-joint-displacement 
imtarg  :  max -joint-displacement 
:accessor  max-joint-displacement))) 


101 


"aqua-data.cl" 


;  load  after  MISC.CL 

(defconstant  *dt*  0.01)  ;  delta-t  each  update, 
(defconstant  *loops*  2)  ;  updates  between  draws. 

(defconstant  linkOlength  37.5) 
(defconstant  linkllength  20.0) 
(defconstant  link21ength  52.0) 
(defconstant  link3 length  102.0) 
(defconstant  flag-length  25.0) 

;  leg  attachment  angles, 
(defconstant  leg  1 -angle  (deg-to-rad  0)) 
(defconstant  leg2 -angle  (deg-to-rad  60)) 
(defconstant  leg3-angle  (deg-to-rad  120)) 
(defconstant  leg4-angle  (deg-to-rad  180)) 
(defconstant  leg5-angle  (deg-to-rad  240)) 
(defconstant  leg6-angle  (deg-to-rad  300)) 

.  initial  position  and  orientation  in  world  coordinates, 
(defconstant  azimuth-irut     (deg-to-rad  0.0)) 
(defconstant  elevation-init  (deg-to-rad  0.0)) 
(defconstant  roll-init        (deg-to-rad  9.0)) 
(defconstant  x-init  0.0) 

(defconstant  y-init  0.0) 

(defconstant  z-imt         -135.0) 

;  initial  (default)  joint  angles. 

(defconstant  joint  1-imt  (deg-to-rad     0.0)) 

(defconstant  joint2-init  (deg-to-rad    25.0)) 

(defconstant  joint3-init  (deg-to-rad  -1 15.0)) 

(defconstant  default-angles  (list  joint  1-init  joint2-init  joint3-init)) 

,  joint  spnng  constants,  (fill  in  :  Kg-cm2/sec2  per  radian) 

(defconstant  joint  1-K  -2000000) ;  5000000  =  Scott's  500  Nm  per  radian. 

(defconstant  joint2-K  -2000000) 

(defconstant  joint3-K  -2000000) 

(defconstant  spring-constants  (list  jointl-K  joint2-K  joint3-K)) 

;  joint  spring  damping  constants,  (fill  in  :  Kg-cm2/sec2  per  radian/sec) 

(defconstant  joint  1-D  -800000)  ;  800000  =  Scott's  80  Nm-sec  per  radian/sec. 

(defconstant  joint2-D  -800000) 

(defconstant  joint3-D  -800000) 

(defconstant  spring-damping-constants  (list  jointl-D  joint2-D  joint3-D)) 
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joint  limits. 

(defconstant  joint  1-min-limit  (deg-to-rad  -61 '  0)) 

(dcfconstant  joint  1-max-limit  (deg-to-rad    60.0)) 

(defconstant  jomt2-nun-limit  (deg-to-rad  -106.6)) 

(defconstant  joint2-max-limit  (deg-to-rad    73.4)) 

(defconstant  joint3-min-limit  (deg-to-rad  -156.4)) 

(defconstant  joint3-ma.\-hmit  (deg-to-rad  23.6)) 
(defconstant  joint  1-min-limit  -50  0) 
(defconstant  joint  1-max-limit  50  0) 
(defconstant  joint2-min-limit  -50  ()) 
(defconstant  joint2-max-limit  50.0) 
(defconstant  joint3-min-limit  -s<>  0) 
(defconstant joint3-max-limit  50.0) 

.  mass  in  Kg. 

(defconstant  aqua-body-mass  500.0) 

;(defconstant  link  1  mass  0.0) 

.(defconstant  link2mass  0.0) 

;( defconstant  link3mass  0.0) 

;  (Ix  Iy  Iz)-Kg-cm.   in  principal  axis  coordinates 
,  assumes  solid  cylindrical  body  of  constant  density 
(defconstant  aqua-body-height  50.0) 
(defconstant  aqua-body-radius  30.0) 
(defconstant  aqua-body-lx 

(+  (*  (/  1  4)  aqua-body-mass  (sqr  aqua-body-radius)) 
(*  (/  1  12)  aqua-body-mass  (sqr  aqua-body-height)))) 
(defconstant  aqua-body-Iy  aqua-body-lx) 

(defconstant  aqua-body-Iz  (*  (/  1  2)  aqua-body-mass  (sqr  aqua-body-radius))) 
(defconstant  aqua-body-inertia  (list  aqua-body-lx  aqua-body-Iy  aqua-body-Iz)) 

;  center  of  mass. 

(defconstant  body-mass-center  '(0  0  0)) 

;(defconstant  link  1  mass-center  (list  (/  linkllength  2)  0  0)) 

;(defconstant  link2mass-center  (list  (/  link21ength  2)  0  0)) 

.(defconstant  link3 mass-center  (list  (/  link31ength  2)  0  0)) 

(defconstant  *gravity*  980.0)  ;cm/sec/sec. 
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;  "aqua-link.cl" 


.  requires  LINK.CL 

.  requires  AQUA-DATA.CL 

(defclass  linkO  (rotary-link) 
((twist-angle  initform  0) 
(link-length  anitform  linkOlength) 
(inboard-joint-angle  initform  0) 
(inboard-joint-displacement  initform  0) 
(min-joint-angle  :irutform  (deg-to-rad  -360)) 
(max-joint-angle  initform  (deg-to-rad  360)) 
(node-list  : initform  (list  (list  0  0  0  1)  (list  0  0  0  1) 

(list  linkOlength  0  0  1)))        for  mdh 

;  (list  (-  linkOlength)  0  0  1)))  .  for  dh 

(polygon-list  :initform  '((1  2))))) 

(defclass  linkl  (rotary-link) 
((twist-angle  initform  (deg-to-rad  -90)) 
(link-length  :imtform  linkllength) 
(inboard-joint-angle  initform joint  1-init) 
(inboard-joint-displacement  :initform  0) 
( min-joint-angle  :  initform  joint  1  -nun-limit) 
( max-joint-angle  :  initform  joint  1  -max-limit) 
(node-list  :imtform  (list  (list  0  0  0  1)  (list  0  0  0  1) 

(list  linkllength  0  0  1)))      ;  for  mdh 

;  (list  (-  linkllength)  0  0  1))) ;  for  dh 

(polygon-list  :initform  '((1  2))))) 

(defclass  link2  (rotary-link) 
((twist-angle  :irutform  0) 
(link-length  initform  link21ength) 
(inboard-joint-angle  initform  joint2-inif) 
(inboard-joint-displacement  :imtform  0) 
(min-joint-angle  imtform joint2-min-limit) 
(max -joint-angle  imtform joint2-max-limit) 
(node-list  imtform  (list  (list  0  0  0  1)  (list  0  0  0  1) 

(list  link21ength  0  0  1)))      ;  for  mdh 

;  (list  (-  link21ength)  0  0  1)));  for  dh 

(polygon-list  :initform  '((1  2))))) 
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(defclass  link3  (rotary-link) 

((twist-angle  mitform  0) 

(link-length  mitform  Hnk31ength) 

(inboard-joint-angle  lmtform  joint3-init) 

(inboard-joint-displacemeiu  mitform  0) 

(min-joint-angie  :irutformjoint3-min-limit) 

(max-joint-angle  uniform  joint3-max-linun 

(node-list  mitform  (list  (list  0  0  0  1)  (list  0  0  0  1 ) 

(list  hnk31ength  0  0  1)))      .  for  mdh 
;  (list  (- link31ength)  0  0  1)))  .  fordh 

(polygon-list  initform  '((1  2))))) 

.;  fordh 

(defmcthod  update-A-matrix  ((link  link)) 
(with-slots  (twist-angle  link-length  inboard-joint-angle 
inboard-joint-displacement  A-matnx)  link 
(serf  A-matrix 

(dh-matrix  (cos  inboard-joint-angle)  (sin  inboard-joint-angle) 
(cos  twist-angle)       (sin  twist-angle) 
link-length  inboard-joint-displacement)))) 

;  added  for  mdh 

(defmethod  update-T-matrix  ((link  link)) 
(with-slots  (twist-angle-i-1  link-length-i-1  inboard-joint-angle 
inboard-joint-displacement  T-matnx)  link 
(setf  T-matnx 

(mdh-matnx  (cos  inboard-joint-angle)  (sin  inboard-joint-angle) 
(cos  twist-angle-i- 1 )  (sin  twist-angle-i- 1 ) 
link-length-i-1      inboard-joint-displacement)))) 

(defmethod  rotate  ((link  link)  angle) 
(setf  (inboard-joint-angle  link)  angle) 
(update-T-matnx  link) 
(setf  (H-matnx  link)  (matrix-multiply  (H-matnx  (inboard-link  link)) 

(T-matnx  link))) 
(transform-node-list  link)) 

(defmethod  rotate-link  ((link  link)  angle) 
(cond  ((>  angle  (max -joint-angle  link)) 
(rotate  link  (max -joint-angle  link)) 
(setf  (motion-limit-flag  link)  t)) 
((<  angle  (min-joint-angle  link)) 
(rotate  link  (min-joint-angle  link)) 
(setf  (motion-limit-flag  link)  t)) 
(t  (rotate  link  angle)  (setf  (motion-limit-flag  link)  ml)))) 
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;  "aqua.cl' 


;  requires  STROBE-CAMERA  CL 
;  requires  AQUA-LEG. CL 

(defclass  aquarobot-body  (ngid-body) 
((mass  initform  aqua-body-mass) 
(moments-of-inertia  initform  aqua-body-inertia) 
(node-list 
:initform  (list  (list  0  0  0  1) 

(list  (*  (cos  legl-angle)  linkOlength) 

(*  (sin  legl-angle)  linkOlength)  0  1) 
(list  (*  (cos  leg2-angle)  linkOlength) 

(*  (sin  leg2-angle)  linkOlength)  0  1) 
(list  (*  (cos  leg3-angle)  linkOlength) 

(*  (sin  leg3-angle)  linkOlength)  0  1) 
(list  (*  (cos  leg4-angle)  linkOlength) 

(*  (sin  leg4-angle)  linkOlength)  0  1) 
(list  (*  (cos  leg5-angle)  linkOlength) 

(*  (sin  leg5-angle)  linkOlength)  0  1) 
(list  (*  (cos  leg6-angle)  linkOlength) 

(*  (sin  leg6-angle)  linkOlength)  0  1) 
(list  linkOlength  0  (-  flag-length)  1))) 
(polygon-list 
:initform'((l  2  3  4  5  6)  (1  7))))) 

(defclass  aquarobot  () 
((body 

initform  (make-instance  'aquarobot-body) 

: accessor  body) 
(legl 

:initform  (make-instance  'aqua-leg  : leg-attachment-angle  legl-angle) 

accessor legl) 
(leg2 

:initform  (make-instance  'aqua-leg  : leg-attachment-angle  leg2-angle) 

: accessor leg2) 
(leg3 

:initform  (make-instance  'aqua-leg  : leg-attachment-angle  leg3 -angle) 

:  accessor leg3) 
(leg4 

initform  (make-instance  'aqua-leg  : leg-attachment-angle  leg4-angle) 

accessor  leg4) 
(leg5 

initform  (make-instance  'aqua-leg  :  leg-attachment-angle  leg5 -angle) 

:accessor  leg5) 
(leg6 

: initform  (make-instance  'aqua-leg  : leg-attachment-angle  leg6-angle) 

:accessor  leg6))) 


106 


idefmethod  world-to-aqua  ((aqua  aquarobot)  xyz-pos) 
(world-to-body  (body  aqua)  xyz-pos)) 

(defmethod  aqua-to-world  ((aqua  aquarobot)  xyz-pos) 
(body-to-world  (body  aqua)  xyz-pos)) 

.(defmethod  initialize  ((aqua  aquarobot)) 
(setf  (H-matrix  (body  aqua)) 

(homogeneous-transform  azimuth-imt  elevation-imt  roll-init 
x-init  y-init  z-init)) 
(transform-node-list  (body  aqua)) 
( update-position  (body  aqua)) 
(setf  (forces-and-torques  (body  aqua))  '(0  0  0  0  odd 
(setf  (acceleration  (body  aqua))  '(0  0  0  0  0  0)) 
(setf  (velocity  (body  aqua))  '(0  0  0  0  0  <))) 
(start-timer  (body  aqua)) 
(nutialize-leg  (legl  aqua)  (body  aqua)) 
(nutialize-leg  (legl  aqua)  (body  aqua)) 
(lnitialize-leg  (leg3  aqua)  (body  aqua)) 
(initialize-leg  (leg4  aqua)  (body  aqua)) 
(initialize-leg  (leg5  aqua)  (body  aqua)) 
(initialize-leg  (leg6  aqua)  (body  aqua))) 

(defun  aqua-picture  () 
(setf  aqua- 1  (make-instance  'aquarobot)) 
(initialize  aqua-1) 

(move-incremental  aqua-1  null-move-list);sets  "prev-foot-pos" 
(setf  camera- 1  (make-instance  'strobe-camera)) 
(take-picture  camera- 1  aqua-1)) 

(defmethod  take-picture  ((camera  strobe-camera)  (aqua  aquarobot)) 
(take-picture  camera  (body  aqua)) 
(take-picture  camera  (legl  aqua)) 
(take-picture  camera  (leg2  aqua)) 
(take-picture  camera  (leg3  aqua)) 
(take-picture  camera  (leg4  aqua)) 
(take-picture  camera  (leg5  aqua)) 
(take-picture  camera  (leg6  aqua))) 

(defun  new-picture  () 
(erase-camera-window  camera- 1) 
(take-picture  camera- 1  aqua-1)) 
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(defmethod  move-incremental  ((aqua  aquarobot)  increment-list) 
(move-incremental  (body  aqua)  (first  increment-list)) 
(move-incremental  (legl  aqua)  (second  increment-list)) 
(move-incremental  (leg2  aqua)  (third  increment-list)) 
(move-incremental  (leg3  aqua)  (fourth  increment-list)) 
(move-incremental  (leg4  aqua)  (fifth  increment-list)) 
(move-incremental  (leg5  aqua)  (sixth  increment-list)) 
(move-incremental  (leg6  aqua)  (seventh  increment-list))) 

(defconstant  null-move-list  '((0  0  0  0  0  0)  (0  0  0)  (0  0  0)  (0  0  0) 
(0  0  0)  (0  0  0)  (0  0  0))) 

(defmethod  feasible-movep  ((aqua  aquarobot)  allowable-sinkage 
allowable-slippage) 

(and  (feasible-movep  (legl  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (leg2  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (leg3  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (leg4  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (leg5  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (leg6  aqua)  allowable-sinkage  allowable-slippage))) 

(defun  restart-aqua  () 
(initialize  aqua-1) 

(move-incremental  aqua-1  null-move-list);sets  "prev-foot-pos". 
(new-picture)) 

.replace  some  rigid-body  functions: 

(defmethod  start-timer  ((body  aquarobot-body)) 
(setf  (current-time  body)  0)) 

(defmethod  get-delta-t  ((body  aquarobot-body)) 
(let*  ((delta-t  *dt*) 

(new-time  (+  (current-time  body)  delta-t))) 
(setf  (current-time  body)  new-time) 
delta-t)) 

(defmethod  update-aquarobot  ((aqua  aquarobot))  ;Euler  integration, 
(let*  ((body  (body  aqua)) 

(delta-t  (get-delta-t  body))) 
(update-acceleration  body) 
(update-velocity  body  delta-t) 
(update-H-matrix  body  delta-t) 
(transform-node-list  body) 
(update-position  body) 
(update-forces-and-torques  aqua)))  ;updates  positions  of  legs 
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;  "aqua-leg.cr 


;  requires  AQUA.CL 

;  requires  AQUA-LINK.  CL 

;  requires  STROBE-CAMERA. CL 

(dcfclass  aqua-leg  () 
((leg-attachment-angle 

initarg  leg-attachment-angle 

accessor  leg-attachment-angle) 
(linkO 

initform  (make-instance  linkoi 

accessor  linkO) 
(link] 

initform  (make-instance  'link  1 ) 

accessor  linkl) 
(link2 

initform  (make-instance  'Iinki) 

:accessor  link2) 
(link3 

initform  (make-instance  'link3) 

accessor  link3) 
(motion-complete-flag 

initform  ml 

:accessor  motion-complete-flag) 
( previous-foot-position 

initform  nil 

:accessor  previous-foot-position) 
(cun-ent-foot-position 

: initform  nil 

:accessor  current-foot-position) 
(foot-contact 

initform  nil 

accessor  foot-contact))) 
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(defmethod  lnitialize-leg  ((leg  aqua-leg)  (body  aquarobot-body)) 
(serf  (foot-contact  leg)  nil) 
(setf  (inboard-link  (linkO  leg))  body) 
(serf  (inboard-link  (linkl  leg))  (linkO  leg)) 
(setf  (inboard-link  (link2  leg))  (linkl  leg)) 
(setf  (inboard-link  (link3  leg))  (link2  leg)) 
.  added  for  mdh 

(setf  (twist-angle-i-1  (linkO  leg))  0) 
(setf  (twist-angle-i-1  (linkl  leg))  (twist-angle  (linkO  leg))) 
(setf  (twist-angle-i-1  (link2  leg))  (twist-angle  (linkl  leg))) 
(setf  (twist-angle-i-1  (link3  leg))  (twist-angle  (link2  leg))) 
(setf  (link-length-i-1  (linkO  leg))  0) 
(setf  (link-length-i-1  (linkl  leg))  (link-length  (linkO  leg))) 
(setf  (link-length-i-1  (Hnk2  leg))  (link-length  (linkl  leg))) 
(setf  (link-length-i-1  (link3  leg))  (link-length  (link2  leg))) 
(set-default-angles  leg)) 

(defmethod  set-default-angles  ((leg  aqua-leg)) 
(rotate-link  (linkO  leg)  (leg-attachment-angle  leg)) 
(rotate-link  (linkl  leg)  jointl-init) 
(rotate-link  (link2  leg)  joint2-init) 
(rotate-link  (link3  leg)  joint3-imt) 
(setf  (previous-foot-position  leg)  nil) 
(setf  (current-foot-position  leg) 

(near  3  (third  (transformed-node-list  (link3  leg)))))) ;  for  mdh 

(defmethod  set-angles  ((leg  aqua-leg)  angle-list) 
(rotate-link  (linkO  leg)  (leg-attachment-angle  leg)) 
(rotate-link  (linkl  leg)  (car  angle-list)) 
(rotate-link  (link2  leg)  (cadr  angle-list)) 
(rotate-link  (link3  leg)  (caddr  angle-list))) 

(defmethod  take-picture  ((camera  strobe-camera)  (leg  aqua-leg)) 
(take-picture  camera  (linkl  leg)) 
(take-picture  camera  (link2  leg)) 
(take-picture  camera  (link3  leg))) 
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(defmethod  move-incremental  ((leg  aqua-leg)  increment-list) 
(rotate-link  (linkO  leg)  (leg-attachment-angle  leg)) 

(rotate-link  (linkl  leg) 

(+  (first  increment-list)  (inboard-joint-angle  (linkl  leg)))) 
(rotate-link  (link2  leg) 

(+  (second  increment-list)  (inboard-joint-angle  (link2  leg)))) 
(rotate-link  ( 1 1  nk 3  leg) 

(+  (third  increment-list)  (inboard-joint-angle  (link3  leg)))) 
(setf  (previous-foot-position  leg)  (current-foot-position  leg)) 
(setf  (current-foot-position  leg) 

(near  3  (third  (transformed-node-list  (link3  leg)))))  .  for  mdh 
(near  3  (first  (transformed-node-list  (link3  leg)))))  ;  for  dh 
(setf  (motion-complete-flag  leg)  (not  (or  (motion-limit-flag  (linkl  leg)) 
(motion-limit-flag  (link2  leg))  (motion-limit-flag  (link3  leg)))))) 

(defmethod  feasible-movep  ((leg  aqua-leg)  allowable-sinkage  allowable-slippage) 
(and  (<=  (third  (current-foot-position  leg))  allowable-sinkage) 
(or  (minusp  (third  (current-foot-position  leg))) 
(minusp  (third  (previous-foot-position  leg))) 
(<=  (vector-length  (vector-slippage  leg))  allowable-slippage)))) 

(defmethod  vector-slippage  ((leg  aqua-leg)) 
(vector-subtract  (rest  (reverse  (previous-foot-position  leg))) 
(rest  (reverse  (current-foot-position  leg))))) 

(defmethod  current-joint-angles  ((leg  aqua-leg)) 
(list  (inboard-joint-angle  (linkl  leg)) 
(inboard-joint-angle  (link2  leg)) 
(inboard-joint-angle  (link3  leg)))) 
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'aqua-inverse- kinematics. cl" 


;  load  after  AQUA-LEG.  CL 
.  load  after  AQUA-DATA.  CL 

(defconstant  L2sqr  (sqr  link21ength)) 
(defconstant  L3sqr  (sqr  link31ength)) 

;  assumptions:  dh  coord  system  for  linkO  of  respective  leg: 
;  origin  at  joint  1, 

x-axis  directed  away  from  center  of  body. 
;  z-axis  aligned  with  body  z-axis; 

;  foot-position  =  '(x  y  z). 

(defun  thetal  (foot-position) 
(if  (<  (car  foot-position)  0) 

(atan2  (-  (car  foot-position))  (-  (cadr  foot-position))) 
(atan2  (car  foot-position)  (cadr  foot-position)))) 

.  assumptions:  dh  coord  system  for  linkl  of  respective  leg: 

;  origin  at  joint2. 

;  x-axis  directed  away  from  joint  1 , 

;  z-&xis  aligned  with  body  z-axis; 

foot-position  =  '(x  y  z); 
;  hyp  =  distance  from  joint_2  to  foot, 

(defun  theta2  (foot-position  hyp  hyp-sqr) 
(-  (acos  (/  (+  L2sqr  hyp-sqr  (-  L3sqr))  (*  2  link21ength  hyp))) 
(if  (<  (car  foot-position)  0) 

(-  pi  (asin  (/  (caddr  foot-position)  hyp))) 
(asin  (/  (caddr  foot-position)  hyp))))) 

;  assumptions:  same  as  for  theta2. 
(defun  theta3  (foot-position  hyp-sqr) 
(-  (acos  (/  (+  L2sqr  L3sqr  (-  hyp-sqr))  (*  2  link21ength  link3 length)))  pi)) 


;  returns  foot  position  with  respect  to  joint  1  in  linkO  coord, 
(defmethod  foot-jointl/linkOcoord  ((leg  aqua-leg)  foot-pos) 
(vector-subtract  (world-to-body  (linkO  leg)  foot-pos) 
(list  linkOlength  0  0))) 

,  returns  foot  position  with  respect  to  joint  2  in  linkl  coord. 
;  given  foot-joint  1/linkOcoord. 
(defun  foot-joint2/linkl coord  (foot-pos) 
(list  (-  (sqrt  (+  (sqr  (car  foot-pos))  (sqr  (cadr  foot-pos))))  linkl  length) 
0  (caddr  foot-pos))) 
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.  returns  list  of  joint  angles  required  for  given  (world  coord)  foot  position, 
(defmcthod  aqua-inv-kin  ((leg  aqua-leg)  foot-position) 
(let*  ((post)  (foot-jointl/linkOcoord  leg  foot-position)) 
(posl  (foot-joint2/linklcoord  posOi) 
(hyp-sqr  (+  (sqr  (car  posl))  (sqr  (caddr  posl  mi 
(hyp  (sqrt  hyp-sqr))) 
(list  (thetal  post)) 

(theta2  posl  h\p  hyp-sqr) 
(theta>  posl  hyp-sqr)))) 


nqua-jacobian.cl" 


(defmethod  jacobian  ((leg  aqua-leg)) 
(let*  ((T01  (+  (leg-attachment-angle  leg) 

(inboard-joint-angle  (linkl  leg)))) 
(SOI  (sin  T01))  (C01  (cos  TOD) 
(T2  (inboard-joint-angle  (link2  leg))) 
(S2  (sinT2))    (C2(cosT2)> 
(T23  (+  T2  (inboard-joint-angle  (link3  leg)))) 
(S23  (sinT23))  (C23  (cosT23)) 
(LI  linkllength)  (L2  link21ength)  (L3  link31ength)) 
(list  (list  (-  (*  (+  LI  (*  L2  C2)  (*  L3  C23))  SOI)) 
(-(*(+(*L2S2)(*L3  S23))C01)) 
(-(*L3  C01  S23))) 
(list  (*  (+  LI  (*  L2  C2)  (*  L3  C23))  C01) 
(-(*(+(*L2  S2)(*L3  S23))S01)) 
(-(*L3  SOI  S23))) 
(listO 

(-(+(*L2C2)(*L3C23))) 
(-  (*  L3  C23)))))) 

(defmethod  inverse-jacobian  ((leg  aqua-leg)) 
(matrix-inverse  (jacobian  leg))) 

(defmethod  foot-to-joint-rates  ((leg  aqua-leg)  dX  dY  dZ) 
(post-multiply  (inverse-jacobian  leg)  (list  dX  dY  dZ))) 

(defmethod  joint-to-foot-rates  ((leg  aqua-leg)  dthetal  dtheta2  dtheta3) 
(post-multiply  (jacobian  leg)  (list  dthetal  dtheta2  dtheta3))) 
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"aquarobot-update-forces-and-torques.cl" 


;  load  after  AQUA-DATA.CL 

.  load  after  AQUA.CL 

;  load  after  AQUA-LEG.  CL 

(defmethod  update-forces-and-torques  ((aqua  aquarobot)) 
(setf  (forces-and-torques  (body  aqua))  '(00000  0))  ;clear  last  cycle, 
(add-leg-forces-and-torques  (legl  aqua)) 
(add-leg-forces-and-torques  (legl  aqua)) 
(add-leg-forces-and-torques  (leg3  aqua)) 
(add-leg-forces-and-torques  (leg4  aqua)) 
(add-leg-forces-and-torques  (leg5  aqua)) 
(add-leg-forces-and-torques  (leg6  aqua))) 

(defmethod  add-leg-forces-and-torques  ((leg  aqua-leg)) 
(if  (or  (foot-contact  leg)  (new-contact  leg)) 
(let*  ((body  (inboard-link  (linkO  leg))) 

(joint-angles  (aqua-inv-kin  leg  (current-foot-position  leg)))) 
(set-angles  leg  joint-angles) 

(let*  ((r  (world-to-body  body  (current-foot-position  leg))) 
(omega  (cdddr  (velocity  body))) 
(foot-velocity     ;  in  body  coordinates 
(vector-add 
(scalar-multiply  -1  (near  3  (velocity  body))) 
(cross-product  r  omega))) 
(torques  (vector-add 
(mapcar  '*  spring-constants 

(vector-subtract  joint-angles  default-angles)) 
(mapcar  '*  spnng-damping-constants 
(post-multiply  (inverse-jacobian  leg)  foot-velocity)))) 
(resultant-force 
(scalar-multiply 
-1  (post-multiply 

(matrix-inverse  (transpose  (jacobian  leg))) 
torques)))) 
(if  (still-in-contact  leg  resultant-force  body) 
(add-forces-and-torques-to-body 
body  r  resultant-force)))))) 

(defmethod  add-forces-and-torques-to-body  ((body  aquarobot-body)  r  f) 
(let  ((torques  (cross-product  r  f))) 
(setf  (forces-and-torques  body) 

(vector-add  (forces-and-torques  body) 
(append  f  torques))))) 
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.update  joint  angles  and  foot  position,  detect  foot  hitting  ground 
(defmethod  new-contact  ((leg  aqua-leg)) 
(move-incremental  leg  '(0  0  0)) 
(if  (>  (third  (current-foot-position  leg))  0) 
(setf  (third  (current-foot-position  leg))  0 

(foot-contact  leg)  t) 
nil)) 

detect  loss  of  contact  (positive/down  z  component  in  world  coord) 
:side  effect  of  reseting  leg  to  default  state  when  nil  is  returned 
(defmethod  still-in-contact  ((leg  aqua-leg)  force/body-xyz 
(body  aquarobot-bodyi) 
(let  ((force/world-xyz  (vector-subtract  (body-to-world  body  force/body-xyz) 
(location  bod\ )))) 
(if  (>  (third  force/world-xyz)  0) 

(and  (set-default-angles  leg)  (setf  (foot-contact  leg)  nil)) 
t))) 
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APPENDIX    D 


OPERATING  INSTRUCTIONS 


Call  "droptest"  with  zero  to  four  arguments. 

First  arg        Spring  Constant  (2. 15,  default  5) 

Second  arg    Damper  Constant(0.5  .15,  default  5) 

Third  arg       Drop  Height  (0..100,  default  0)  cm 

Fourth  arg     Update  Time  Increment  ( 10. .50.  default  50)  ms 


SOURCE  CODE  FILES 

//file"droptest.c" 


/* 

/*  droptest. c 

/* 

/*    performer  Aquarobot  model  with  "spring"  joints. 

/* 

/*  */ 

#include<stdlib.h> 
#include  <stdio.h> 
#include  <string  h> 
#include  <math.h> 
#include  <gl/device.h> 

/*  performer  */ 
#include  "pf.h" 

/*  performer  aqua-robot  object  constructor  */ 
#include  "pfaqua.h" 

/*  physical  aqua-robot  object  constructor  and  controls  */ 
#include  "aqua.H" 

static  void  OpenPipeline  (pfPipe  *p); 
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void 

main  (int  argc,  char  *argv(]) 

{ 

pfPipe       *p; 

pfChannel    *chan; 

pfScene      *scene, 

pfDCS        *robot_position; 

pfGroup     *aqua_robot;      /*  graphics  object  (performer)  */ 

pfDCS        *JoinlDCS[6][4|; 

aquarobot    robot.  /*  physical  object  */ 

//  defaults  for  args 

float  spring  =  SPRINGJC; 

float  damp    =  SPRJNGJD; 

float  height  =  O.Of; 

float  step    =  0.05f;  //  default  -  real  time 

//  process  args 

if  (argc  >  1)  {  //  spring  constant 
//  first  arg:  -15,000,000  <=  spnng  <=  -2.000.000 
spring  =  fabs((float)(atoi(argv[l]))); 
if  (spnng  <  2  Of) 

spnng  =  2. Of; 
else  if  (spnng  >  15. Of) 

spring  =  15. Of; 
spnng  *=  -1000000; 

} 

if  (argc  >  2)  {  //  spring  damping  constant 
//  second  arg:  -1.500,000  <=  damp  <=  -  50,000 
damp  =  fabs((float)(atoi(argv[2]))); 
if  (damp<0.5f) 
damp  =  0.5f; 
else  if  (damp  >  15. Of) 

damp  =  15. Of; 
damp  *= -100000; 

} 
if(argc>3){ 

//  third  arg:  0  <=  drop  height  <=  100 
height  =  fabs((float)(atoi(argv[3]))); 
if  ( height  >  100. Of) 
height  =  100. Of; 

} 

if  (argc  >  4)  { 

//  fourth  arg:  10ms  <=  integration  time  step  <=  50ms 
step  =  fabs((float)(atoi(argv[4])))/1000  Of; 
if(step<0.01f) 

step  =  0.0  If; 
else  if  (step  >  0.05f) 

step  =  0.05f; 
} 
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/*  1.  initialize  Performer  */ 
pflnitO; 

/*  2.  configure  MP  mode  and  start  parallel  processes  */ 
pfConfigO; 

/*  3.  load  scene  database  */ 
scene  =  pfNe\vScene(); 

robot_position  =  pfNe\vDCS(); 
pfAddChildtscene,  robot_position); 
aqua_robot  =  MakeAquaRobot(JointDCS); 
pf AddChild( robot_position,  aqua_robot); 

/*  5.  configure  and  open  full-screen  pipeline  */ 

p  =  pfGetPipe(O); 

pf!nitPipe(p,  OpenPipeline);  /*  pf!nitPipe(p,  NULL);  */ 

/*  set  frames  per  second  (  if  step  =  .05  sec,  then  ~  real  time)  */ 
pfFrameRate(20.0f); 

/*  6.  get  and  configure  channel  */ 
chan  =  pfNewChan(p); 
pfChanScene(chan.  scene); 
pfChanNearFarfchan,  O.lf,  lOOO.Of); 
pfChanFOV(chan,  45. Of,  -l.Of); 

/*  zero  clock  (not  really  needed)  */ 
pflnitClockO; 

/*  initialize  robot  */ 

robot. imtialize( spring,  damp,  height); 

updatejointDCS(robot,  JointDCS); 

/*  set  up  view  position  */ 

pfCoord  view; 

pfSetVec3(view.hpr,  O.Of,  30.0f,  180.0f); 

pfSetVec3(view.xyz,  O.Of,  -500.0f,  -350.0f); 

pfChanView(chan,  view.xyz,  view.hpr); 
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/*  7.  create  rendering  loop  */ 

/*  simulate  for  30  seconds  */ 

int  t  =  0; 

while  (t  <  600)  //  -  20  frames  per  second 

{ 
/*  Transfer  robot  data  to  graphics  object  */ 
pfDCSMatnx(robot_position.  robot  bod\  H  matrix).   *  body    */ 
updatejointDCSf  robot.  JointDCS).  /*  joints  */ 

/*  Go  to  sleep  till  next  frame  time  */ 
pfSync();  t++; 

/*  initiate  cull/draw  for  this  frame  */ 

pfFrameO; 

pfDrawChanStats(chan); 

/*  Move  robot  to  new  position.  */ 
robot  update_aquarobot(step); 
} 

/*  8.  terminate  parallel  processes  and  exit  */ 
pfE.xitO; 
exit(0); 
} 
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/* 

*  OpenPipelineO  --  create  a  pipeline:  setup  the  window  system. 

*  the  IRIS  GL,  and  IRIS  Performer.  This  procedure  is  executed  in 

*  the  draw  process  (when  there  is  a  separate  draw  process). 

*/ 

static  void 
OpenPipeline  (pfPipe  *p) 

pfLight  *Sun; 

/*  negotiate  with  window -manager  */ 
foreground! ), 
prefposition(0.600.0.600). 
winopen("Aqua  Drop"); 

/*  negotiate  with  GL  */ 
pflrutGfx(p); 

/*  create  a  light  source  */ 

Sun  =  pfNewLight(pfGetSharedArenaO); 

pfLightPos(Sun,  O.Of,  O.Of,  l.Of.  0.00; 

/*  create  a  default  lighting  model  */ 
pfApplyLModel(pfNewLModel(pfGetSharedArena())); 
pfLightOn(Sun); 
} 
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//file"pf_aqua.h" 

/* 

*  pf_aqua.h 

* 

*  call  "MakeAquaRobot"  to  make  AquaRobot  performer  object. 

*  JointDCS(i|(j]  points  to  pfDCS  for  leg  i.  joint  j. 

*  where  j  =  0  is  the  shoulder  joint,  andj  =  3  attaches  the  foot 

* 

*/ 

^include  "pf  h" 

pfGroup* 

MakeAquaRobottpfDCS  *JointDCS[6][41); 


//fileMpf_aqua.c" 


/* 

*  pfaqua.c 

*  call  "MakeAquaRobot"  to  make  AquaRobot  performer  object. 

* 

*/ 

^include  "pfaqua.h" 
^include  "aqualink.H" 

/*  polygon  data  for  aquarobot  */ 
#include  "polybody.h" 
^include  "polyshoulder.h" 
#include  "polyupperleg  h" 
#include  "polylowerleg.h" 
#include  "polyfoot.h" 

/*  geostate  for  multiple  parts  */ 
static  pfGeoState  *robotyellow_gstate; 
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pfGeoSet* 
MakeBodyGSet(void) 

{ 

pfGeoSet  *gset; 
void  *arena; 
pfMatenal  *mtl; 

arena  =  pfGetSharedArenaQ; 
gset  =  pfNewGSet(arena); 

/*  set  the  coordinate  and  normal  arrays  */ 

PfGSetAttr(gset.  PFGS_COORD3.  PFGS_PER_VERTEX,  bodycoords,  NULL); 

pfGSetAttr(gset,  PFGS_NORMAL3.  PFGS_PER_PRIM,  bodynorms,  NULL); 

pfGSetPnmType(gset.  PFGS_QUADS); 
pfGSetNumPnmsfgset,  94); 

/*  set  up  geostate  for  "robotyellovv"  matenal  */ 

robotyelIow_gstate  =  pfNewGState(arena); 

mtl  =  pfNewMtl(arena); 

pfMtlColortmtl.  PFMTL_ AMBIENT.  0.2f.  0.2f.  O.Of); 

pfMUColor(mtl,  PFMTL_DIFFUSE,  l.Of.  1. Of,  O.Of); 

pfMtlColor(mtl,  PFMTL_EMISSION.  O.Of.  O.Of.  O.Of); 

pfMUColor(mtl,  PFMTL_SPECULAR,  O.Of.  O.Of.  O.Of); 

pfMtlAlpha(mtl,  1.0); 

pfGStateAttr(robotyellow_gstate,  PFSTATE_FRONTMTL.  mtl); 

pfGSetGState(gset,  robotyellow_gstate); 

return  gset; 
} 

pfGeoSet* 
MakeLink  1  GSet(void) 

{ 
pfGeoSet  *gset; 
void      *arena; 

arena  =  p  f Get  Shared  Are  na(); 
gset  =  pfNewGSet(arena); 

pfGSetAttr(gset,  PFGS_COORD3,  PFGS_PER_ VERTEX,  link  1  coords,  NULL); 
pfGSetAttr(gset,  PFGS_NORMAL3,  PFGS_PER_PRTM,  linklnorms,  NULL); 

pfGSetPnmType(gset,  PFGS_QUADS); 
pfGSetNumPrims(gset.  42); 

pfGSetGState(gset,  robotyellowgstate); 

return  gset; 
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pfGeoSet* 
MakeLink2GSet(void) 

{ 
pfGeoSet  *gset; 
void      *arcna; 

arena  =  pfGetSharedArenaf); 
gset  =  pfNewGSet(arena); 

pfGSctAttr(gset.  PFGSCOORD3.  PFGS_PER_  VERTEX.  Hnk2coords.  NULL). 
pfGSctAttngset.  PFGS_NORMAL3.  PFGS  PERPRIM.  lmk2norms.  NULL): 

pfGSctPrimTvpc(gset.  PFGS_QUADS); 
pfGSetNumPnms(gset,  91); 

pfGSetGStatefgset,  robotyello\v_gstate); 

return  gset; 
} 

pfGeoSet* 
MakeLink3GSet(void) 

{ 
pfGeoSet  *gset; 
void      *arena; 

arena  =  pfGetSharedArena(); 
gset  =  pfNewGSet(arena); 

pfGSetAttr(gset.  PFGS_COORD3,  PFGS_PER_VERTEX1  link3coords.  NULL); 
pfGSetAttr(gset,  PFGS_NORMAL3,  PFGS_PER_PRIM,  link3norms,  NULL); 

pfGSetPnmType(gset,  PFGS_QUADS); 
pfGSetNumPnms(gset.  103); 

pfGSetGState(gset,  robotvellowgstate); 

return  gset; 


123 


pfGeoSet* 
MakeFootGSet(void) 

{ 

pfGeoSet  *gset; 
void      *arena; 

arena  =  pfGetSharedArena(); 
gset  =  pfNewGSeu  arena); 

pfGSetAttr(gset.  PFGS_COORD3.  PFGS_PER_VERTEX.  footcoords.  NULL); 
pfGSetAttr(gset.  PFGS_NORMAL3,  PFGS_PER_PRIM.  footnorms,  NULL); 

pfGSetPnmType(gset.  PFGS_TRJSTRIPS), 
pfGSetNumPnms(gset,  49); 
pfGSetPnmLengths(gset,  footstnplengths); 

pfGSetGStatefgset,  robotyellowgstate); 

return  gset; 


pfGeoSet* 
MakeShaftGSet(void) 

{ 

pfGeoSet    *gset; 

void        *arena; 

pfGeoState  *robotgray_gstate; 

pfMaterial  *mtl; 

arena  =  pfGetSharedArena(); 

gset  =  pfNewGSet(arena); 

pfGSetAttr(gset,  PFGS_COORD3,  PFGS_PER_VERTEX,  shaftcoords,  NULL); 
pfGSetAttr(gset,  PFGS_NORMAL3,  PFGS_PER_PRIM,  shaftnorms,  NULL); 

pfGSetPnmType(gset,  PFGS_QUADS); 
pfGSetNumPrims(gset,  20); 

/*  set  up  material  */ 

robotgraygstate  =  pfNewGState(arena); 

mtl  =  pfNewMtl(arena); 

pfMUColor(mU,  PFMTL_AMBffiNT,  0.1,  0.1,  0.1); 

pfMtlColor(mtl,  PFMTL_DIFFUSE,  0.2,  0.2,  0.2); 

pfMtlColor(mtl,  PFMTL_EMISSION,  0.0,  0.0,  0.0); 

pfMUColor(mU,  PFMTL_SPECULAR,  0.0,  0.0,  0.0); 

pfMtlAlpha(mtl,  1.0); 

pfGStateAttr(robotgray_gstate,  PFSTATEFRONTMTL,  mtl); 

pfGSetGState(gset,  robotgray_gstate); 


return  gset; 


} 
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pfGroup* 

MakeAquaRobot(pfDCS  *JoiniDCS[6|[4|) 

{ 
pfSCS        *LegAttachSCS[6], 

*LinklSCS[6],  *Link2SCS[6],  *Lmk3SCS[6], 

*FootSCS|6|. 
pfMatrix     rotmat.  trans  mat; 

pfGroup      "AquaRobotGroup.  *BodyGroup[6|.  *LcgGroup|6| 
pfGcode      *geode_body. 

*gcode_linkl.  *geode_lmk2.  *geode_link3, 

*geode_shaft.  *geode_foot; 
int  i;  /*  loop  counter  */ 

/*  make  geodes  */ 
geodebody  =  pfNewGeodeO. 
pfAddGSet(geode_body.  MakeBodyGSetO); 

geodelinkl  =  pfNewGeodeO; 
pfAddGSeKgeodelink  1 .  MakeLink  1  GSetO): 

geode_link2  =  pfNewGeodeO; 
pfAddGSet(geode_link2.  MakeLink2GSet( )); 

geode_link3  =  pfNewGeodeO; 
pfAddGSet(geode_link3.  MakeLink3GSet()); 

geode_foot  =  pfNewGeodeO; 
pfAddGSet(geode_foot,  MakeFootGSetO); 

geodeshaft  =  pfNewGeodeO; 
pfAddGSet(geode_shaft,  MakeShaftGSetO); 

/*  Make  Parent  Group  */ 
AquaRobotGroup  =  pfNewGroupO; 

/*  Add  Structure  (6  segments)  */ 
for(i  =  0;  i<  6;  i++) 

{ 
/*  rotate  to  segment  */ 

pfMakeRotMat(rot_mat,  i*60.0,  0.0,  0  0,  10); 
LegAttachSCS[i]  =  pfNewSCS(rot_mat); 
pfAddChild( AquaRobotGroup,  LegAttachSCS(il); 

/*  add  body  slice  */ 
BodyGroup[i]  =  pfNewGroupO; 
pfAddChild(LegAttachSCS[i),  BodyGroup|i|); 
pfAddChild(BodyGroup[i],  geodebody). 
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} 


/*  add  leg  */ 

/*  link  1  */ 

pfMakeTransMat(trans_mat,  LINKOLENGTH.  0.0.  0.0); 

LinklSCS[i]  =  pfNe\vSCS(trans_mat); 

pfAddChild(BodyGroup(i],  LinklSCS[i]); 

pfAddChild(LinklSCS[i],geode_shaft). 

JointDCS[i][0]  =  pfNewDCSO; 
pfAddChild(LinklSCS[i],  JointDCS[il[0]); 
pfAddChild(JointDCS[i][0],  geodejinkl); 

/*  link  2  */ 

pfMakeRotMat(rot_mat,  -90.0.  1.0.  0.0.  0.0); 

pfMakeTransMat(trans_mat.  LINK1LENGTH.  0.0.  0.0); 

pfPostMultMat(rot_mat,  trans_mat); 

Link2SCS[i]  =  pfNe\vSCS(rot_mat); 

pfAddChild(JointDCS[i][0].  Link2SCS[i]); 

pfAddChiId(Link2SCS[i].geode_shaft); 

JointDCS[i][l]  =  pfNewDCSO; 
pfAddCruld(Link2SCS[i],  JointDCS(i][l]); 
pfAddChild(JointDCS[i] [  1  j,  geode_hnk2); 

/*  link  3  */ 

pfMakeTransMat(trans_mat.  LINK2LENGTH,  0.0,  0.0); 
Link3SCS[i]  -  pfNewSCS(trans_mat); 
pfAddChild(JointDCS[i][l|.  Link3SCS[i]); 
pfAddChild(Link3SCS[i],  geode_shaft); 

JointDCS[i][2]  =  pfNewDCSO; 
pfAddChild(Link3SCS[i],  JointDCS[i][2]); 
pfAddChild(JointDCS[i][2],geode_link3); 

/*  foot  */ 

pfMakeTransMat(trans_mat,  LINK3LENGTH,  0.0,  0.0); 
FootSCS[i]  =  pfNewSCS(trans_mat); 
pfAddChild(JointDCS[i][2],  FootSCS[i]); 

JointDCS[i][3]  =  pfNewDCSO; 

pfAddChild(FootSCS[i],  JointDCS[i][3]); 

pfAddChild(JointDCS[i][3],  geode_foot); 
} 
return  AquaRobotGroup; 
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//file"aqua.h" 


#ifndef_AQUA_H 
#define  _AQUA_H 

^include  <Performer/pf.h> 
••'include  "misc.H" 
^include  "ngid_bod>  H" 
^include  "aquaJeg.H" 

typedef  ngidbody  aquarobotbody . 

/*  mass  in  Kg.  */ 

#define  AQUA_BODY_MASS  500  Of 

/*  (Ix  Iy  Iz)-Kg-cm    in  principal  axis  coordinates.  */ 
/*  assumes  solid  cylindrical  body  of  constant  density    */ 
#define  AQUABODYHEIGHT  50  of 
#define  AQUA_BODY_RADIUS  30.0f 
static  pfVec3  aquabody   inertia  =  { 
//Ix 

1  Of/4. Of  *  AQUABODYMASS  *  AQUA_BODY_RADIUS  *  AQUA_BODY_RADIUS 
+  1. Of/ 12. Of  *  AQUA_BODY_MASS  *  AQUA_BODY_HEIGHT  *  AQUA_BODY_HEIGHT. 
//  Iy 

1. Of74  Of  *  AQUA_BODY_MASS  *  AQUA_BODY_RADRJS  *  AQUA_BODY_RADIUS 
+  1.0f712.0f  *  AQUABODYMASS  *  AQUABODYHEIGHT  *  AQUA_BODY_HEIGHT, 
II  lz 
1. Of/2. Of  *  AQUA_BODY_MASS  *  AQUA_BODY_RADIUS  *  AQUA_BODY_RADIUS}; 

/*  tmtial  position  and  orientation  in  world  coordinates.  */ 

#define  AZIMUTHJNIT     deg_to_rad(0.0f) 

#define  ELEVATION_INIT  deg_to_rad(0.0f) 

#define  ROLL_INIT        deg_to_rad(0.0f) 

#define  X_INTT  O.Of 

#define  Y_INIT  O.Of 

#define  Z_INIT  sinf(defauIt_angles[l])*LINK2LENGTH  -  LINK3LENGTH 

/*  leg  attachment  angles.  */ 

#define  LEG  1  ANGLE  deg_to_rad(O.Of) 

#define  LEG2ANGLE  deg_to_rad(60.0f) 

#define  LEG3  ANGLE  deg_to_rad(120.0f) 

#define  LEG4 ANGLE  deg_to_rad(180.0f) 

#define  LEG5ANGLE  deg_to_rad(240.0f) 

#define  LEG6ANGLE  deg_to_rad(300.0f) 
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class  aquarobot 

{ 
public: 

aquarobotbody  body, 
//private: 

aqua_leg        legl,  leg2,  leg3.  Ieg4,  leg5,  leg6; 

private: 
void 
aquarobot:  :initjoint_access(); 

void 
update_forces_and_torques( ); 

void 
update_legs(); 

public: 
//  External  access  to  joint  angles  for  passing  to  performer  model 
//  This  could  be  private  if  "updatejointDCS"  were  a  friend; 
//  however,  the  class  should  not  depend  on  needs  of  user, 
float*      joint_matrix[6][4]; 

public: 
aquarobot():body(AQUA_BODY_MASS,  aquabodyinertia), 
legl  (LEG  1  ANGLE), 
leg2(LEG2ANGLE), 
leg3(LEG3  ANGLE), 
leg4(LEG4  ANGLE), 
leg5(LEG5  ANGLE), 
leg6(LEG6 ANGLE)  { init Joint_access(); } 

void 

initialize(float  k  =  SPRINGJC,  float  d  =  SPRJNG_D,  float  height  =  O.Of); 

void 

update_aquarobot(float  dt  =  O.Of); 

//  coordinate  transformation  routines 
void 

world_to_aqua(pfVec3  destination,  pfVec3  source) 
{body.world_to_body(destination,  source); } 

void 

aqua_to_world(pfVec3  destination,  pfVec3  source) 
{body. body _to_world(destination,  source); } 

}; 
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void 

updatejointDCSCaquarobot  robot.  pfDCS  *JointDCS(6)(4|); 

#endif 
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//  File  "aqua.c' 


^include  "aqua.H" 
#include  <math.h> 

/*  user  routines  */ 

void 

aquarobot::initialize(float  k,  float  d.  float  height) 

{ 

body.move(AZIMUTH_INIT.  ELEVATIONINIT.  ROLL_INIT. 
X_INIT.  YJNIT,  -fabs(height)+Z_INIT); 

pfSetVec3(body.vel_trans.  O.Of,  O.Of.  O.Of); 

pfCopyVec3(body.vel_rot.  body.veltrans); 

pfCopyVec3(l)ody.accel_trans.  body.vel_trans); 

pfCop\Vec3(l5ody.accel_rot,  body.veltrans); 

pfCopyVec3 (body. forces,  body.veljrans); 

pfCop\Vec3(body. torques,  body.veltrans); 

body.start_timer(); 

legl.irut_leg(&body,  k,  d); 

leg2.init_leg(&body,  k.  d); 

leg3  init_leg(&body,  k,  d); 

leg4  init_leg(&body,  k,  d); 

leg5.ini t_leg(&body.  k,  d); 

leg6.ini t_leg(&body,  k,  d); 

update_forces_and_torques(); 
} 

void  aquarobot::update_aquarobot(float  dt) 

{ 

float  dt_  =  dt; 
if(dt<=0.0) 
dt_  =  body.get_delta_t();  //  default 

body.update_acceleration(); 

body  update_velocity(dt_); 

body.update_H_matrix(dt_); 

body.update_position(dt_); 
//  body.update_velocity(dt_); 

updatelegsO; 

/*  This  is  done  last  as  it  also  updates  leg  positions:  */ 

/*  leg  positions  depend  on  "new"  body  position!         */ 

updatefo  rces_and_to  rquesO ; 
} 
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/*  Internal  Routines  */ 

void 

aquarobot::init  Joint  jiccessQ 

{ 

//  for  use,  see  fn: "update jointDCS"  below 
joint_matrix[0][0]  =  &legl  link  1  inboard  jointangle. 
joint_matn\[o||  1  ]  =  &legl.link2  inboard  jointangle. 
joint_matrix[0j[2]  -  &legl  link3  inboardjointangle. 
joint_matnx|0]|3]  =  &legl.link4.H_matrix[0j[0]; 

joint_matrix[l][0]  =  &ieg2.1inkl  inboardjointangle; 
joint  niatn.\[  1 1(  1|  =  &leg2  link2  .inboard  Joint  jingle; 
joint  matnx[l][2]  =  &leg2.1ink3  inboardjointangle; 
joint_matrix[l][3]  =&leg2.Iink4.H_matrixl()|[0|: 

jointmatnx[2)[0]  =  &leg3.1inkl.inboardjoint_angle; 
joint  jnatnx[2 1[1]  =  &leg3.1ink2  inboard  jointangle; 
joint  matnx[2][2]  =  &leg3.1ink3  inboardjointangle; 
joint_matrix[2][3]  =  &leg3.1ink4.H_matrix[0][0]; 

jointjiiatnx[3|[0|  =  &leg4.  link  1  inboardjoint  jingle. 
jointjnatnx[3)[l)  =  &leg4.1ink2. inboardjoint  jingle; 
joint_matnx[3|[2]  =  &leg4  link3  inboardjointangle; 
joint_matrix[3][3]  =  &leg4.1ink4.H_matnx[0][0]; 

joint_matnx[4][0|  =  &leg5.1inkl  inboardjointangle; 
joint_matnx|4|[l|  =  &leg5.1ink2.inboardjoint_angle; 
joint_matnx[4][2|  =  &leg5  1ink3.  inboard  jointangle; 
joint jnatnx[4)[3]  -  &leg5.1ink4.H_matrix[0][0); 

joint_matnx[5|[0]  =  &leg6  link  1.  inboard  jointangle; 
joint_matnx[51[l)  =  &leg6.1ink2.inboardjoint_angle; 
joint_matnx[5][2|  =  &leg6.1ink3  inboard  jointangle. 
joint  _matrix[5](3]  =  &leg6.1ink4.H_matrix[0][0); 
} 

void  aquarobot:  :update_forces_and_torques() 

{ 

pfSetVec3 (body. forces.  O.Of,  O.Of,  0  Of); 

pfSetVec3 (body. torques,  O.Of,  O.Of,  O.Of); 

legl.add_leg_forces_and_torques(); 

leg2 .  add_leg_forces_and_torques( ) ; 

leg3.add_leg_forces_and_torques(); 

leg4  add  legforcesand  torques( ) ; 

leg5  addlegforcesand  torques(); 

leg6 .  addlegforcesand  torques( ) ; 
} 
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void  aquarobot::update_legs() 
{ 

legl  update_leg(); 

leg2  update_leg() 

leg3.update_leg() 

leg4.update_leg() 

leg5  updatelegO 

leg6  updatelegO 


/*  joint  angle  transfer  routine  */ 


\oid 


update JointDCS(aquarobot  robot.  pfDCS  *JointDCS[6][4D 


static  pfMatnx  m4  =  {{0,0,0,0}, {0,0,0,0}, {0,0,0.0}, {0,0.0.1}}; 
for(int  i=0;i<6;i++)  { 
//  rotate  first  three  joints 
for(intj=0;j<3;j++)  { 
pfDCSRot(JointDCS[i](j]. 

rad_to_deg(*robot.joint_matnx[i][j]),  O.Of,  O.Of); 

} 

//  eqiuv  to  rot(0.\,-90y,0z)  *  inverse(leg[i].link4.H_Matrix) 

//  accomplishes  DCS  such  that  link(x-axis)  ||  world(z-axis) 


m4[0][0]  =  (robot.joint_matrix[i 
m4[0][l]=  (robot.joint_matrix[i 
m4[0)[2]  =  (robot.joint_matrix[i 
m4[l][0]=  (robot.joint_matrix[i 
m4[l][l]=  (robot.joint_matrix[i 
m4[l][2]=  (robot.joint_matrix[i 
m4[2][0]  =  -(robot.joint_matrix[i 
m4[2][l]  =  -(robot.joint_matrix[i 
m4[2|[2]  =  -(robot.joint_matrix[i 
pfDCSMatri.\(JointDCS[i][3],  m4 


3])[2]; 
3])[6]; 

3])[10] 

3])[1] 
3])[5] 
3])[9] 

3])[0] 
3])[4] 
3])[8]; 
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//file"aqiia_leg.hM 


#ifndef_AQUA_LEG_H 
#define  _AQUA_LEG_H 

^include  <Performer/pf  h> 
^include  "ngidbody  H" 
^include  "aqua_link.H" 
^include  "misc.H" 

/*  initial  (default)  joint  angles.  */ 
static  pfVec.l  dcfaultangles  =  { 

/*     0  deg  */     O.Of. 

/*    45  deg*/    25. Of*  PI_F  /  180. Of. 

/*  -135  deg  */-115.0f  *  PIF  /  180.0f}; 

/*  joint  spring  constant,  (default  :  5,000,000  Kg-cm2/sec2  per  radian)  */ 
^define  SPRINGK  -5()()0U()0  of 

/*  joint  spring  damping  constant,  (default  :  500, 000  Kg-cm2/sec2  per  radian/sec)  */ 
^define  SPRING_D  -500000.0f 

/*  AQUA  LEG  CLASS  */ 

class  aqualeg 

{ 
public: 

float       legattachmentangle; 

aqualinkO  linkO; 

aqualinkl   linkl; 

aqualink2  link2; 

aqualink3   link3; 

aqualink4  link4; 

boolean     motioncompleteflag; 

pfVec3      previous_foot_position; 

pfVec3      current_foot_posiuon; 

boolean     footcontact; 

float       sprk; 

float       sprd; 

public: 
aqua_leg(float  angle  =  O.Of):  legattachmentangle(angle) 

{ 

motioncompleteflag  =  TRUE; 

footcontact  =  FALSE; 
} 

void 

init_leg(rigid_body  *body,  float  k  =  SPRTNGJC,  float  d  =  SPPJNG_D); 
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void 
set_default_angles(), 

void 
update_leg(); 

void 
update_foot_pos(); 

void 

set_angles(float  joint  1.  float  joint2,  float  joint3); 

void 

set_angles(pfVec3  angles)  {set_angles(angles[Oj.  angles[l|.  angles[2]);} 

void 
jacobianfpfMatnx  J); 

void 

inverse jacobian(pfMatnx  J_inv); 

void 

joint_rates(pfVec3  rates); 

void 

FootPosFmJointl(pfVec3  foot_posJl,  pfVec3  foot_pos_\vorld); 

void 

aqua_invJan(pfVec3  joint_angles,  pfVec3  worldfoot  jdos); 

void 
add_leg_forces_and_torques(); 

int 
new_contact(); 

int 

still_in_contact(pfVec3  legforcebody); 
}; 

#endif 
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//file  "aqua  leg.c" 

^include  <math.h> 
^include  "aqua_leg  H" 

/*  AQUA-ROBOT  INVERSE  KINEMATICS  ROUTINES  */ 

static  float  L2sqr  =  LINK2LENGTH  *  LINK2LENGTH; 
static  float  L3sqr  =  LINK3LENGTH  *  LINK^LENGTH: 

/*  routines  that  return  the  joint  angles  for  a  leg.  gi\en  the  foot  position  */ 

float 

thetal(pfVec3  foot_pos) 

if(foot_pos[U]  <0.0f) 

return  (atan2f(-foot_pos|l|,  -foot_pos(0])); 
else 

return  (atan2f(  foot_pos|  1|,  foot_pos[0])h 
} 

float 

theta2(pfVec3  foot_pos.  float  hvp.  float  hvpsqr) 

{ 

float  temp  -  asinf(foot_pos|2]/h\p); 

if  (foot_pos[0J  <  0  Of)  temp  -  PIF  -  temp; 

return  (acosf((L2sqr  +  hvp_sqr  -  L3sqr)  /  (2  *  LINK2LENGTH  *  h\p))  -  temp  ); 
} 

float 

theta3(float  hvpsqr) 

{ 
return  (acosf((L2sqr  +  L3sqr  -  hvp_sqr)  /  (2  *  LINK2LENGTH  *  LINK3LENGTH))  -  PIF); 

} 

/*  supports  theta2  and  theta3  which  require  foot  position  with  respect  */ 

/*  to  joint2  position.  joint2  position  depends  on  thetal.  */ 

void 

FootPosFmJoint2(pfVec3  destination,  pfVec3  source) 

{ 

destinauon(O)  =  sqrtf(source[0]*source[0]  +  source(l)*source[l|) 
-  LINK  1  LENGTH; 

destination^]  =  O.Of; 

destination[2]  =  source[2|; 
} 
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/*  AQUA  LEG  CLASS  */ 

void 

aqua_leg::mit_leg(ngid_body  *body,  float  k.  float  d) 

{ 

sprk  =  k;  sprd  =  d; 

foot_contact  =  0; 

link!)  inboardjink  =  body; 

linkl  inboardjink  =  &linkO; 

link2.  inboardjink  =  &linkl. 

Iink3  inboardlink  =  &link2; 

link4.inboard_link  =  &link3; 

set_default_angles(); 
} 

void 

aqualeg:  :set_default_angles() 

» 

set_angles(default_angles); 

update_foot_pos(); 

pfCopyVec3(previous_foot_position,  current_foot_position); 
} 

void 
aqua_leg::update_leg() 

{ 

pfCopyVec3(previous_foot_position,  current_foot_position); 
set_angles(  link  1  inboard  jointangle, 
link2  inboard  jointangle, 
link3.  inboard  jointangle); 
update_footjpos(); 
} 


void 

aqualeg:  :update_foot_pos() 

{ 

if  (Ifootcontact)  { 
current  foot_position[0]  =  link4.H_matrix[3](0J 
currentfoot  jpositionf  1  ]  =  link4.H_matrix[3)[lJ 
current_foot_position[2]  =  link4.H_matrix[3][2] 

} 
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void 

aquaJeg::set_angles(float  joint  1.  float  joint2.  float  joint3) 

! 

linkO  rotatelinkdegattachmcntangle). 

linkl  rotatejink(jointl); 

Iink2  rotate_link(joint2), 

link.3  rotate_link(joint3); 
//  this  works  for  each  leg  in  2D,  but  world  3D  solution  requires! '  i  0) 
//  link4.rotate_link(-deg_to_rad(90  of)  -joint2  -joint  J) 

link4  rotate  link(O.Of); 


void 
aqua_leg::jacobian(pfMatn.\  J) 

{ 

pfVec3  row; 

float  angle.  SOI,  C01.  S2.  C2.  S23.  C23; 

#defineLl  LINK  1  LENGTH 

#define  L2  LINK2LENGTH 

#dcfine  L3  LINK3LENGTH 

angle  =  leg_attachment_angle  +  link  1  inboard  jomtangle; 

SOI  =  sinf(angle); 

C01  =  cosf(angle); 

angle  =  link2.inboardjoint_angle, 

S2  =  sinf(angle); 

C2  =  cosf(angle); 

angle  +=  link3.  inboard  jointangle; 

S23  =  sinf(angle), 

C23  =  cosf(angle); 

pfMakeldentMat(J); 

pfSetVec3(row,  -SOI  *  (LI  +  L2*C2  +  L3*C23), 

-C01  *(L2*S2  +  L3*S23), 

-C01  *L3  *  S23); 
pfSetMatColVec3(J,  0,  row); 
pfSetVec3(row,  C01  *  (LI  +  L2*C2  +  L3*C23), 

-SOI  *(L2*S2  +  L3*S23), 

-SOI  *L3  *  S23); 
pfSetMatColVec3(J,  1,  row); 
pfSetVec3(row,  O.Of, 

-L2*C2-L3*C23, 

-L3  *  C23); 
pfSetMatColVec3(J,  2,  row); 
} 


137 


void 

aqua_leg: : inverse jacobian(pfMatnx  J_inv) 

{ 

pfMatnx  J; 

jacobian(J); 

pflnvertMat(J_inv,  J); 
} 

void 

aqua  leg:  :joint_rates(pfVec3  rates) 

{ 

pfMatnx  J_inv, 

pfVec3  trans_rates; 

pfVec3  omega; 

pfVec3  foot_r; 

pfVec3  rot_rates; 

pfVec3  foot_rates; 

inverse  jacobian(J_inv); 

pfScaleVec3 (translates,  -l.Of,  ((rigid_body  *)linkO.  inboard  Jink)->vel_trans); 

pfCopyVec3  (omega,  ((ngidbody  *)linkO.  inboard  Jink)->vel_rot); 

((ngidbody  * )linkO. inboard  link)->world_to_body(foot_r.  current_foot_position); 

pfCrossVec3(rot_rates,  omega,  foot_r); 

pfSubVec3(  footrates,  transrates,  rot_rates); 
post_mult(rates,  Jinv,  footrates); 
} 

void 

aqua_leg::FootPosFmJointl(pfVec3  foot_posJl,  pfVec3  foot_pos_world) 

{ 
linkO  world_to_body(foot_posJ  1,  foot_pos_world); 
foot_posjl[0]  -=  LINKOLENGTH; 

} 

void 

aqua_leg::aqua_inv_kin(pfVec3  jointangles,  pfVec3  world_foot_pos) 

{ 

pfVec3  footjointl,  footJoint2; 

float  hyp,  hyp_sqr; 

FootPosFmJoint  1  (foot  Joint  1 ,  world_foot_pos); 

FootPosFmJoint2(footJoint2,  footjointl); 

hyp  =  pfLengthVec3(footJoint2); 

hypsqr  =  hyp  *  hyp; 

pfSet  Vec3  (jointangles,  theta  1  (foot  Joint  1 ), 

theta2(footJoint2,  hyp,  hypsqr), 
theta3(hyp_sqr)); 
} 
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void 

aquajeg:  :add_leg_forces_and_torques( ) 

{ 
if  (foot_contact  ||  new_contact()) 

{ 
pf\ec3    angles,  jointtorques.  damp,  forces,  foot_pos; 
pfMatrix  workmatnxl.  uork_matnx2; 

aqua_inv_kin(angles,  currcnt_foot_positionK 
set_angles(angles). 

/*  get  spring  force  of  joints  */ 
pfSubVec3(joint_torques.  angles,  default  angles). 
pfScaleVec3(joint_torques.  sprk.  jointtorques); 

/*  add  damping  */ 
jointrates(damp); 
pfScaleVec3(damp,  sprd.  damp), 
pfAddVec3(joint_torques.  jointtorques.  damp). 

jacobian(\vork_matnx  1 ); 
pfTransposeMat(\vork_matn\2.  workmatnxl); 
pflnvertMat(\vork_matnxl,  \vork_matnx2); 
post_mult(forces,  workmatnxl,  jointtorques); 
pfScaleVec3 (forces,  -1.0,  forces); 

if  (still_in_contact(  forces)) 

{ 

((ngidbody  *)link0.inboard_link)->world_to_body(foot_pos,  cunent_foot_position); 
((rigidbody  *)link0  inboard_link)->add_force_and_torques(foot_pos.  forces); 
} 
} 
} 

int 

aqualeg : :  new  _contact( ) 

{ 
if  (cunent_foot_position[2]  >  O.Of) 

{ 

cunent_foot_position[2]  =  O.Of; 
return  (footcontact  =  TRUE); 

} 
else 

return  FALSE; 
} 
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int 

aqua_leg::still_in_contact(pfVec3  leg_force_body) 

{ 

pfVec3  leg_force_\vorld; 

((ngidbody  *)link0.inboard_link)->body_to_\vorld(leg_force_\vorld.  leg_force_body); 
pfSubVec3(leg_force_vvorld,  leg_force_\vorld.  ((ngid_body  *)linkO  inboard_link)->location); 
if(leg_force_world[2]  >  0  Of) 

{ 

set_default_angles(); 

return  (foot_contact  =  FALSE), 
> 

else 
return  TRUE; 
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//file  "aqualink.h" 

#ifndef  _AQUA_LINK_H_ 
#define  _AQUA_LINK_H_ 

^include  <Performer/pf.h> 
^include  "ngidbody  H" 

/*  BASE  CLASSES  */ 

class  linkpublic  ngidbody 
/ 

public: 

inl  motion_limit_flag; 

float  lcngthil. 

float  twistil; 

float  inboardjoint_angle; 

float  inboard  jointdisplaccment; 

void*  inboardlink; 
pfMatrix  Tmatnx; 

public: 
link(float  mass  =  1  Of.  pfVec3  moments  =  NULL):rigid_body(mass.  moments){ } 
void  update_T_matrix(); 
void  rotate( float  angle); 

}; 

class  rotarvlinkpublic  link 

{ 
public: 

float  minjointangle; 

float  maxjointangle; 

public: 
rotary_link(float  length  =  O.Of, 

float  minangle  =  O.Of, 

float  maxangle  =  O.Of, 

float  twist  =  O.Of, 

float  jointangle         =  O.Of, 

float  jointdisplacement  =  O.Of, 

void*  inboardlink        =  0); 
void  rotate_link(float  angle); 

}; 
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/*  MODIFIED  DANEVIT-HARTENBERG  LINK  COORDINATE  TRANSFORMATION  MATRIX 

void 
mdh_matn\(pfMatn.\  mdh, 

float  cosrotate,     float  sinrotate. 

float  costwist_i_l,  float  sintwist_i_l, 

float  lengthil.    float  translate); 


/*  AQUA-LINK  CLASSES  */ 

/*  link  lengths  */ 
#define  LINKOLENGTH  37.5f 
#define  LINKILENGTH  20  Of 
#define  LINK2LENGTH  52. Of 
#define  LINX3 LENGTH  102. Of 
#define  LINK4LENGTH  3. Of 

/*  joint  limits  */ 

#define  JOINT  1MIN  deg_to_rad(  -60.0f) 
ffdefine  JOINT  1  MAX  deg_to_rad(  60.0f) 
#define  JOINT2MIN  deg_to_rad(-360.0f) 
#define  JOINT2MAX  deg_to_rad(  360.0f) 
#define  JOINT3MIN  deg_to_rad(-360.0f) 
#define  JOINT3MAX  deg_to_rad(  360.0f) 
#define  JOINT4MIN  deg_to_rad(  -22. 5f) 
#define  JOINT4MAX  deg_to_rad(  22.5f) 

class  aqualinkOpublic  rotarylink 

\ 

public: 
aqualinkO(); 

}; 

class  aqualinkLpublic  rotary_link 

public: 
aqualinkl(); 
}- 

class  aqualink2:  public  rotarylink 

{ 

public: 
aqualink20; 

}; 
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class  aqualink3  public  rotarvjink 

{ 
public 

aqualink3(); 


class  aqualink4  public  rotarv_link 

\ 

public: 
aqualink4(); 

}; 

#endif 
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//  file  "aquajink.c" 


#include  <math.h> 
^include  "aquaJink.H" 
#include  "misc.H" 

/*  BASE  CLASSES  */ 

void 
link::update_T_matrix() 

{ 

float  sa  =  sinf( inboard joint_angle); 
float  ca  =  cosf(inboardjoint_angle); 
float  st  =  sinf(t\vist_i_l); 
float  ct  =  cosf(fvvist_i_l); 

mdh_matnx(T_matnx,  ca,  sa,  ct,  st,  lengthil.  inboard  jointdisplacement). 
} 

void 

link::rotate(float  angle) 

{ 

inboardjointangle  =  angle; 

update  T_matnx(); 

//  multiplied  in  reverse  order  as  they  are  stored  as  transposes. 

pfMultMat(H_matrix,  Tmatrix,  ((ngidbody  *)inboard_link)->H_matrix); 
} 

rotarv_link::rotary_link(float  length, 
float  minangle, 
float  maxangle, 
float  twist, 
float  jointangle, 
float  jointdisplacement, 
void*  inboard_link_) 

{ 

lengthil  =  length; 

minjointangle  =  minangle; 

maxjointangle  =  maxangle; 

twist_i_l  =  twist; 

inboard  Jointangle         =  jointangle; 

inboard  Jointdisplacement  =  jointdisplacement; 

inboard  link  =  inboard_link_; 

pfMakeldentMat(Tmatrix); 
} 
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void 

rotarvlink:  rotate_link(float  angle) 

{ 

/*  joint  stops  disabled 
if  (angle  <  minjoint_angle) 

{ 

rotate!  nun  joint_angle). 
motion_limit_flag  =  -1; 

} 
else  if  (angle  >  maxjointangle) 

{ 
rotate(maxJoint_angle); 

motion_limit_flag  =  1; 

else 


rotate(angle); 
motionlimitflag  =  0; 


} 


*/ 

} 


void 

mdhmatnx 
float 
float 
float 

{ 

/♦col 
mdh(0 
mdh[l 
mdh[2 
mdh[3 
/*  col  2 
mdh[0 
mdh[l 
mdh(2 
mdh[3 
I*  col  3 
mdh[0 
mdh[l 
mdh|2 
mdh(3 
/*  col  4 
mdh[0 
mdh(3 


(pfMatnx  rndh, 
cosrotate,     float  sinrotate, 
costwistil.  float  sintwist_i_l, 
lengthil,    float  translate) 


cosrotate; 

-  sinrotate; 
O.Of; 
length_i_l; 

sinrotate  *  costwistil; 
cosrotate  *  costwistil; 

-  sintwist_i_l; 

-  sintwist_i_l  *  translate; 

sinrotate  *  sintwist_i_l; 
cosrotate  *  sintwist_i_l; 
costwistil; 
costwist  i  1  *  translate; 


mdh[l][3]  =  mdh[2]I3] 
l.Of; 


O.Of; 
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/*  alternate  method  using  pf  functions 
pfVec3    col; 
pfMakeldentMat(mdh); 
pfSetVec3(col,  cosrotate,  sinrotate  *  cost\vist_i_  1 . 

sinrotate  *  sintwist_i_l); 
pfSetMatRowVec3(mdh.  0,  col); 
pfSetVec3(col,  -sinrotate,  cosrotate  *  costwist_i_l, 

cosrotate  *  sint\vist_i_l); 
pfSetMatRowVec3(mdh,  1.  col); 
pfSetVec3(col,  0.0,  -sintwistil .  costwist  i_l), 
pfSetMatRo\vVec3(mdh,  2.  col); 
pfSetVec3(col.  lengthil.  -  sintwistil  *  translate. 

costwistil  *  translate); 
pfSetMatRo\vVec3(mdh.  3.  col); 
*/ 
} 

/*  AQUA-LINK  CLASSES  */ 

aqualinkO:  :aqualinkO() 
{ 
maxjointangle  =  deg_to_rad(360.0f); 
} 

aqualink  1 :  :aqualink  1  ( ) 

{ 
length_i_l       -  LINKOLENGTH; 
rrunjointangle  =  JOINT  1MIN; 
maxjointangle  =  JOINT  1  MAX; 

} 

aqualink2 :  :aqualink2() 

{ 

length  j_l       =  LINK1LENGTH; 

twistil        =  deg  to_rad(-90.0f); 

rrunjointangle  =  JOINT2MIN; 

maxjoint_angle  =  JOINT2MAX; 
} 

aqualink3 :  :aqualink3() 

{ 
length_i_l       =  LINK2LENGTH; 
rrunjointangle  =  JOINT3MIN; 
maxjointangle  -  JOINT3MAX; 

} 
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aqualink4::aqualink4() 


lengthjj        =  LINK3  LENGTH. 
minjoint_angle  =  J0INT4MIN. 
maxjomtangle  =  JOINT4MAX. 

} 
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//file"rigid_body.H" 


#ifndef  _RJGID_BODY_ 
#define  _RIGID_BODY_ 

^include  <Performer/pf.h> 

#define  GRAVITY  980.0 

class  ngid_body 

{ 

public: 
pfVec3  location;    /*  The  vector  (x  y  z)  in  world  coordinates.  */ 
pfVec3  veltrans;  /*  The  vector  (u  v  w)  in  body  coordinates.  */ 
pfVec3  vel_rot;     /*  The  vector  (p  q  r)  in  body  coordinates.  */ 
pfVec3  acceltrans;/*  The  vector  (u-dot  v-dot  w-dot).  */ 
pfVec3  accel_rot;  /*  The  vector  (p-dot  q-dot  r-dot).  */ 
pfVec3  forces;      /*  The  vector  (Fx  Fy  Fz)  in  body  coordinates.  */ 
pfVec3  torques;     /*  The  vector  (L  M  N)  in  body  coordinates.  */ 
pFVec3  moments;    /*  The  vector  (Ix  Iy  Iz)  in  principal  axjs  coordinates.  */ 
float  mass_;       /*  in  Kg.  */ 
float  currenttime; 
pfMatnx  Hmatrix; 

public: 
ngid_body( float  mass,  pfVec3  moments  =  NULL); 

void 

move(float  azimuth,  float  elevation,  float  roll, 
float  x,        float  y,  float  z); 

float 
get_delta_t(); 

void 
start_timer(); 

/* 
void 

updaterigidbodyO; 

*/ 

void 
update_acceleration(); 

void 

update_velociry  (float  dt); 
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void 

update_H_matnx(  float  dt); 

void 
update_position(float  dt); 

void 

\vorld_to_bod\(pfYec3  destination.  pfVec3  source). 

void 

body_to_uorld(pfVec3  destination.  pfVec3  source); 

void 

add_force_and_torques(pfVec3  r.  pfV'ec3  f): 


}: 


void 

homogeneotis_transform(  pfMatnx  homo. 

float  azimuth,  float  elevation,  float  roll. 

float  x.        float  y.  float  z); 

void 

post_mult(pfVec3  destination.  pfMatnx  m.  pfVec3  source). 

#endif 
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//fileMrigid_body.CM 


#include  "ngid_body.H" 
#include  "misc.H" 

ngid_body::ngid_body(float  mass,  pfVec3  moments) 

{ 
location    [0]  =  location    [1]  =  location    [2]  = 
veltrans  [0]  =  vel_trans  [1]  =  veltrans  [2]  = 
vel_rot     [0]=vel_rot     [l]=vel_rot     [2]  = 
accel_trans[0]  =  accel_trans[l]  =  accel_trans[2]  = 
accelrot  [0]  =  accel_rot  [  1  ]  =  accel_rot  [2|  = 
forces      [0]  =  forces      [1]  =  forces      [2]  = 
torques     [0]  =  torques     [1]  =  torques     [2]  =  O.Of; 
if  (moments  ==  NULL) 

moments_[0]  =  moments_[l]  =  moments_[2]  =  O.Of; 
else 

pfCopyVec3(  moments,  moments); 
mass_  =  mass; 
pfMakeldentMat(Hmatrix); 
cuiTent_time  =  O.Of; 
} 

void 

ngid_body::move(float  azimuth,  float  elevation,  float  roll, 
float  x,        float  y,  float  z) 

i 

homogeneous_transform(H_matrix,  azimuth,  elevation,  roll,  x,  y,  z); 

pfSetVec3( location,  x,  v,  z); 
} 

float 

ngidbody :  :get_delta_t() 

{ 

float  dt  =  0.05f; 

curTenttime  +=  dt; 

return  dt; 
} 

void 

ngidbody:  :start_timer() 

{ 
currenttime  =  O.Of; 

} 
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/* 

void 

ngidbody : :  update_ngid_bodv( ) 

{ 

float  dt  =  getdeltat; 
update_H_matnx(  dt ) ; 
update_position(dt); 
update_velocit\(dn. 
update_acceleration( ); 

} 

*/ 

void 

ngidbody:  :update_acceleration( ) 

{ 
accel_trans|0)  =  veltrans(l)  *  \el_rot|2]  -  vel_trans|2]  *\el_rot[l| 

+  forces[0)  /  mass_         +  GRAVITY  *  H_matrix[0)[2]; 
accel_trans(l]  =  vel_trans|2]  *  vel_rot|o]  -  vel_trans|0]  *  vel_rot[2] 

+  forces[  1]  /  mass_  +  GRAVITY  *  H_matn.\[l][2]; 

accel_trans[2|  =  vel_trans[0|  *  \el_rot[l]  -  vel_trans(l|  *  vel_rot|<)| 

+  forces[2]  /  mass_  +  GRAVITY  *  H_matnx[2][2]; 

accel_rot[0|    = 

((moments_[l|  -  moments_[2])  *  velrotjl]  *  \cl_rot|2]  +  torqucs(0|) 
/  moments_[0|; 
accel_rot[l]    = 

((moments_[2]  -  moments_[0|)  *  vel_rot[2|  *  vel_rot{0]  +  torques)  1|) 
/  moments!  1], 
accel_rot[2]    = 

((momentsJO]  -  moments_[l|)  *  vel_rot|0|  *  vel_rot[l]  +  torques[2|) 
/  moments_[2|, 
} 

void 

ngidbodv:  updatevelocitv  (float  dt) 

{ 

pfVec3  dv; 

pfScaleVec3(dv,  dt,  acceltrans); 

pfAddVec3(vel_trans,  veltrans,  dv); 

pfScaleVec3(dv,  dt,  accelrot); 

pfAddVec3(vel_rot,  velrot,  dv); 
} 
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void 

ngid_bodv:  :update_H_matnx(float  dt) 

{ 

pfMatnx  homo; 

homogeneous_transform(homo,  dt  *  vel_rot[2], 
dt  *  vel_rot[l], 
dt  *  vel_rot[0), 
dt  *  vel_trans[0], 
dt  *  vel_trans[l], 
dt  *  vel_trans[2]); 

pfPreMultMat(H_matn.\,  homo); 


void 

ngid_body   update_position( float  dt) 

{ 
pfGetMatRo\vVec3(H_matrix,  3.  location); 

< 

void 

rigid_body::\vorld_to_body(pfVec3  destination,  pfVec3  source) 


pfMatnx  inv_H; 
pflnvertMatdnvH,  Hmatrix); 
post_mult(destination,  inv_H,  source); 


void 

ngid_body::body_to_world(pfVec3  destination,  pfVec3  source) 

{ 
post_mult(destination,  Hmatrix,  source); 

void 

rigid_bodv::add_force_and_torques(pfVec3  r,  pfVec3  f) 

{ 

pfVec3  t; 

pfCrossVec3(t,  r,  f); 

pfAddVec3 (torques,  torques,  t); 

pf Add Vec3  (forces,  forces,  f); 
} 


152 


void 

homogeneoustransfortiKpfMatnx  homo. 

float  azimuth,  float  elevation,  float  roll. 

float  x.        float  v.  float/.) 


{ 


float  sz.  cz,  sv.  cv,  sx,  ex; 


pfSinCos(rad_to_deg(a/imuth).  &sz.  &cz); 
pfSinCos(rad_to_deg(elevation).  &sy.  <fec>) 
pfSinCos(rad_to_deg(roll).  &sx.  &cx); 
/*  col  1  */ 


homo|<> 

[0] 

=  cz*cy. 

homo|  1 

("1 

=  cz*sy*sx  -  sz* 

ex. 

homo  1 2 

[0] 

=  cz*sy*cx  +  sz. 

"sx. 

homo  (3 

[0] 

=  x; 

/*  col  2 

*/ 

homo(() 

hi 

=  sz*cy; 

homo[l 

[i] 

=  cz*cx  +  sz*s> 

"sx; 

homo[2 

[i] 

=  sz*sy*cx  +  cz 

"sx; 

ho  mo  [3 

in 

=  y; 

/*  col  3 

*/ 

honio(i) 

[2] 

-  -sy; 

homo[l 

[2] 

=  c\*sx; 

homo[2 

[2] 

=  cy*cx; 

homo  ( 3 

[2] 

=  z; 

/*  col  4 

*/ 

homo[0 

[3] 

=  homo[l][3]  = 

tiorr 

homo(3 

[3] 

=  l.Of; 

homo[2][3]  =0.0f; 


void 

post_mult(pfVec3  destination.  pfMatnx  m,  pfVec3  source) 

{ 

destination^)]  =  source[0]  *  m[0][0]  +  source[l]  *  m[l][01  + 
source[2]  *  m[2][0)  +  m[3][0); 

destination!  1]  =  source[0]  *  m[0|[l]  +  source|l|  *  m[l|[l]  + 
source[2)  *m[2][l]  +  m[31(l|; 

destination[2]  =  source[0]  *  m[0](2]  +  source[l]  *  m[l|[2]  + 
source[2]  *  m[2][21  +  m[3|[2|; 

} 
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//fileMmisc.HM 


#ifndef_MY_MISC_ 
#define  _MY_MISC_ 

/*  t\pe  BOOLEAN  V 

t>pedef  int  boolean; 

#ifndefTRUE 
#define  TRUE  1 
#endif 

#ifndef  FALSE 
#define  FALSE  0 
#endif 

#definePI    3.14159265358979323846 
#definePI_F3.14159f 

/*  angle  measurement  conversions  */ 

float 

deg_to_rad( float  deg); 

float 
rad_to_deg(float  rad); 

double 
deg_to_rad(double  deg); 

double 
rad_to_deg(double  rad); 

#endif 
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//file  "misc.C" 

#include  "misc.H" 

/*  angle  measurement  conversions  */ 

float 

deg_to_rad(  float  deg) 

{ 

const  float  rad_per_deg  =  PI  F  /  180  Of 
return  (deg  *  rad_per_deg); 

float 
rad_to_deg(float  rad) 

v 

const  float  deg_per_rad  =  180. Of  /  PIJF; 

return  (rad  *  deg_per_rad); 
} 

double 

deg_to_rad( double  deg) 

{ 

const  double  rad_per_deg  =  PI  /  180.0; 

return  (deg  *  rad_per_deg); 
} 

double 
rad_to_deg(double  rad) 

{ 

const  double  deg_per_rad  =  180.0  /  PI; 

return  (rad  *  deg_per_rad); 
} 
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